注：本文为 "卡尔曼滤波 " 相关合辑。
图片清晰度受引文原图所限。
略作重排，如有内容异常，请看原文。
卡尔曼滤波理论讲解与应用（MATLAB 和 Python）
O_MMMM_O 原创于 2020-04-23 22:10:53 发布
本文循序渐进地阐述卡尔曼滤波的基础原理，包含适用前提、理论概述与公式的完整推导过程。通过数理表达阐释基于均值与方差的物体状态描述方法，以及依托状态转移矩阵结合外部控制量完成的状态预测逻辑。本文同时详细说明引入系统扰动与观测值实现预测值修正的方法，并通过高斯分布的融合推导完成最优状态估计。文末提供 MATLAB 与 Python 双语言的工程实现代码，可支撑理论的理解与工程化实践。
理论讲解
适用前提
卡尔曼滤波属于线性滤波算法，其功能为对多元含噪的非确定信息进行融合，最终输出系统的最优状态估计值。卡尔曼滤波在连续变化的线性系统中具备优异的滤波效果，其原因是该算法充分考量了系统运行过程中存在的两类扰动项：模型预测噪声 Q Q Q 与观测过程噪声 R R R。因此，即便系统中存在上述扰动，卡尔曼滤波器仍可对系统的真实状态完成高精度求解，同时能够对系统的未来运动状态做出合理的预判。
卡尔曼滤波器的适用条件为：系统必须是线性高斯系统，该定义可拆解为两个维度的要求，具体如下：
- 线性性：系统的状态方程、观测方程均满足线性关系；
- 高斯性：系统的过程噪声与观测噪声均服从零均值的高斯分布。
综上，高斯分布的噪声项经过线性的状态转移后，其分布形式将保持高斯特性不变。若待处理系统不满足线性条件，则需要采用扩展卡尔曼滤波 (Extended Kalman Filter，EKF) 与无迹卡尔曼滤波 (Unscented Kalman Filter，UKF) 完成替代求解。
理论概述
如下图 2.15 所示，卡尔曼滤波器的信息来源分为两个维度：其一为传感器采集的车辆状态观测值 z ^ k \hat{z}_k z^k，其二为基于数学模型推演的车辆状态预测值 x ^ k \hat{x}_k x^k。两类信息均为系统状态的间接表征，且均伴随非确定性与测量偏差；若对全部有效信息源进行融合，则可得到精度优于任一单一信息源的状态估计值 x ^ k ′ \hat{x}_k' x^k′。
卡尔曼滤波的信息融合本质为两个独立高斯分布的乘积运算 ：传感器的观测值 z ^ k \hat{z}_k z^k 服从某一高斯分布，模型的预测值 x ^ k \hat{x}_k x^k 同样服从某一高斯分布，将二者相乘将得到一个新的高斯分布 x ^ k ′ \hat{x}_k' x^k′。该新分布的取值区间为原两个高斯分布的重叠域，其均值即为系统的最优状态估计值。
卡尔曼滤波的本质，是将预测的状态分布 x ^ k \hat{x}_k x^k 与观测的状态分布 z ^ k \hat{z}_k z^k 两个高斯分布相乘，得到表征最优估计的新高斯分布 x ^ k ′ \hat{x}_k' x^k′。该完整的滤波流程可归纳为两组共五个基础公式：
预测公式
x ^ k = F k x ^ k − 1 + B k u ⃗ k P k = F k P k − 1 F k T + Q k \begin{aligned} \hat x_k&=F_k \hat x_{k-1} + B_k \vec u_k \\ P_k&=F_kP_{k-1}F^T_k+Q_k \end{aligned} x^kPk=Fkx^k−1+Bku k=FkPk−1FkT+Qk
x ^ k \hat{x}k x^k ： k k k 时刻的预测状态向量（预测阶段得到的状态估计）；
F k F_k Fk ：状态转移矩阵，描述无控制、无噪声时， k − 1 k-1 k−1 时刻状态到 k k k 时刻状态的线性映射关系；
x ^ k − 1 \hat{x}{k-1} x^k−1 ： k − 1 k-1 k−1 时刻的最优状态估计（上一时刻更新后的状态）；
B k B_k Bk ：输入控制矩阵，将控制量的作用映射到状态向量的变化上；
u ⃗ k \vec{u}k u k ：系统的控制输入向量，如加速度、角加速度等外部输入物理量；
P k P_k Pk ： k k k 时刻预测状态的协方差矩阵，表征预测状态的估计不确定性；
P k − 1 P{k-1} Pk−1 ： k − 1 k-1 k−1 时刻最优状态的协方差矩阵（上一时刻更新后的协方差）；
F k T F^T_k FkT ：状态转移矩阵 F k F_k Fk 的转置矩阵；
Q k Q_k Qk ：过程噪声协方差矩阵，量化模型未建模因素（干扰、非线性误差）的不确定性。
更新公式
K k = P k H k T ( H k P k H k T + R k ) − 1 x ^ k ′ = x ^ k + K k ( z k ⃗ − H k x ^ k ) P k ′ = P k − K k H k P k \begin{aligned} K_k &= P_kH^T_k\left(H_kP_kH^T_k+R_k\right)^{-1} \\ \hat x'_k &= \hat x_k+K_k\left(\vec {z_k}-H_k\hat x_k\right) \\ P'_k&=P_k-K_kH_kP_k \end{aligned} Kkx^k′Pk′=PkHkT(HkPkHkT+Rk)−1=x^k+Kk(zk −Hkx^k)=Pk−KkHkPk
K k K_k Kk ：卡尔曼增益，权衡"预测状态不确定性"与"观测不确定性"的权重系数；
H k T H^T_k HkT ：观测矩阵 H k H_k Hk 的转置矩阵；
H k H_k Hk ：观测矩阵，将状态向量映射到观测向量的空间（统一维度与物理意义）；
R k R_k Rk ：观测噪声协方差矩阵，表征传感器自身测量误差的不确定性；
z ⃗ k \vec{z}_k z k ： k k k 时刻传感器的观测向量，即传感器的直接测量结果；
x ^ k ′ \hat{x}'_k x^k′ ： k k k 时刻的最优状态估计（更新阶段得到的最终状态）；
P k ′ P'_k Pk′ ： k k k 时刻最优状态的协方差矩阵（更新阶段得到的最终协方差）。
公式推导
1 基于均值与方差的物体状态描述
对任意时刻的物体状态进行量化表征，其状态向量 x ^ k \hat{x}_k x^k 可包含多个物理量，代表待表征的全部系统特征量。我们采用均值与方差作为物体状态的描述指标 ，该设定的理论依据如下：
均值代表待估计变量的状态（如位置、速度、温度等），其物理意义为随机分布的中心值，即系统最可能的真实状态；方差表征变量取值偏离均值的程度，能够反映变量间的关联规律------通过某一变量的取值，推导其余变量的可能取值区间。例如在运动学模型中，位置与速度存在强相关性：基于上一时刻的位置估计当前位置时，速度越大则位置的变化量越大，速度越小则位置的变化量越小。
对该类变量间的相关性进行量化的矩阵，即为协方差矩阵 。协方差矩阵中的任意元素 Σ i j \Sigma_{ij} Σij，表征第 i i i 个状态量与第 j j j 个状态量之间的相关程度。协方差矩阵具备对称矩阵 的数学特性，即满足 Σ i j = Σ j i \Sigma_{ij}=\Sigma_{ji} Σij=Σji。协方差矩阵的通用符号为 Σ \Sigma Σ，其元素记为 Σ i j \Sigma_{ij} Σij。
因此， k k k 时刻物体的完整状态可表示为：状态均值 x ^ k \hat{x}k x^k 与协方差矩阵 P k P_k Pk，其二维运动学表达形式为：
x ^ k = [ p o s i t i o n v e l o c i t y ] P k = [ Σ p p Σ p v Σ v p Σ v v ] \hat{x}k=\begin{bmatrix} position \\ velocity \end{bmatrix} \quad\quad P_k=\begin{bmatrix} \Sigma{pp} & \Sigma{pv} \\ \Sigma_{vp} & \Sigma_{vv} \end{bmatrix} x^k=[positionvelocity]Pk=[ΣppΣvpΣpvΣvv]
2 基于状态转移矩阵 F F F 的系统预测
基于 k − 1 k-1 k−1 时刻的系统状态，完成对 k k k 时刻系统状态的推演，是卡尔曼滤波预测阶段的逻辑。该状态推演过程，可通过状态转移矩阵 F k F_k Fk 完成数学表达。
状态转移矩阵 F k F_k Fk 的构建依据为运动学、动力学等物理规律，其作用是将上一时刻的状态估计值，映射至当前时刻的预测位置；若上一时刻的状态估计无偏差，则该预测位置即为系统的真实下一状态。
以基础匀速运动模型为例，推导状态转移矩阵的构建方法，其运动学公式为：
p k = p k − 1 + Δ t ⋅ v k − 1 v k = v k − 1 \begin{aligned} p_k& = p_{k-1}+\Delta t \cdot v_{k-1} \\ v_k &= v_{k-1} \end{aligned} pkvk=pk−1+Δt⋅vk−1=vk−1
将其转化为矩阵表达形式，可得：
x ^ k = [ 1 Δ t 0 1 ] x ^ k − 1 = F k x ^ k − 1 \hat x_k=\begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \hat x_{k-1} = F_k \hat x_{k-1} x^k=[10Δt1]x^k−1=Fkx^k−1
完成状态向量的预测后，需同步完成协方差矩阵的更新。针对随机变量的线性变换，存在明确的协方差矩阵变换规律：若随机变量 x x x 的协方差为 Σ \Sigma Σ，即 C o v ( x ) = Σ \mathrm{Cov}(x)=\Sigma Cov(x)=Σ，则对其做线性变换 A x Ax Ax 后的协方差为：
C o v ( A x ) = A Σ A T \mathrm{Cov}(Ax)=A\Sigma A^T Cov(Ax)=AΣAT
结合状态转移的公式，可得预测阶段的协方差矩阵更新公式：
x ^ k = F k x ^ k − 1 P k = F k P k − 1 F k T \hat x_k=F_k \hat x_{k-1} \\ P_k = F_k P_{k-1} F^T_k x^k=Fkx^k−1Pk=FkPk−1FkT
3 引入外部控制变量 u ⃗ k \vec{u}_k u k
实际工程场景中，系统的状态变化除自身演化规律外，还会受到外部控制量的主动干预，这类控制量与系统自身状态无直接关联。例如：火车的加速源于司机对油门的控制，机器人的转向源于导航系统的指令输入。若该类外部控制量已知，则可构建控制向量 u ⃗ k \vec{u}_k u k，并将其引入状态预测方程完成修正，提升预测精度。
以匀加速运动模型为例，已知系统的期望加速度为 a a a，其运动学公式更新为：
p k = p k − 1 + Δ t ⋅ v k − 1 + 1 2 a ( Δ t ) 2 v k = v k − 1 + a ⋅ Δ t \begin{aligned} p_k& = p_{k-1} + \Delta t \cdot v_{k-1} + \frac{1}{2} a (\Delta t)^2 \\ v_k &= v_{k-1} + a \cdot \Delta t \end{aligned} pkvk=pk−1+Δt⋅vk−1+21a(Δt)2=vk−1+a⋅Δt
转化为矩阵表达形式为：
x ^ k = F k x ^ k − 1 + [ ( Δ t ) 2 2 Δ t ] a = F k x ^ k − 1 + B k u ⃗ k \hat x_k = F_k \hat x_{k-1}+\begin{bmatrix} \frac{(\Delta t)^2}{2} \\ \Delta t \end{bmatrix} a =F_k \hat x_{k-1} + B_k \vec u_k x^k=Fkx^k−1+[2(Δt)2Δt]a=Fkx^k−1+Bku k
式中， B k B_k Bk 为控制矩阵 ， u ⃗ k \vec{u}_k u k 为控制向量 。对于无外部控制的简单系统，该修正项可直接省略。
4 引入过程噪声 Q k Q_k Qk
基于系统自身规律与外部控制量完成的状态预测，仅适用于无未知扰动的理想场景。实际工程中，系统普遍存在不可建模的外部扰动：例如四旋翼飞行器会受风力干扰、轮式机器人会出现地面打滑、车辆会因路面坡度产生速度变化。若忽略该类扰动，状态预测结果将出现显著偏差。
卡尔曼滤波对该类未知扰动的处理方式为：将其建模为零均值的高斯噪声 ，其协方差矩阵记为 Q k Q_k Qk。该噪声项表征了模型未覆盖的不确定性，其物理意义为： k − 1 k-1 k−1 时刻的状态经转移后，其分布范围将因未知扰动而扩大，形成协方差为 Q k Q_k Qk 的新高斯分布。
由此，卡尔曼滤波预测阶段的完整公式 为：
x ^ k = F k x ^ k − 1 + B k u ⃗ k P k = F k P k − 1 F k T + Q k \hat x_k=F_k \hat x_{k-1} + B_k \vec u_k \\ P_k=F_kP_{k-1}F^T_k+Q_k x^k=Fkx^k−1+Bku kPk=FkPk−1FkT+Qk
该公式的物理意义可总结为： k k k 时刻的状态预测值 x ^ k \hat{x}_k x^k，由上一时刻的最优估计值经状态转移与外部控制修正后得到； k k k 时刻的预测协方差 P k P_k Pk，由上一时刻的协方差经矩阵变换后，叠加过程噪声的协方差得到。
至此，我们得到了系统状态的先验估计（均值 x ^ k \hat{x}_k x^k + 协方差 P k P_k Pk），下一步将引入传感器的观测值，完成先验估计的修正。
5 基于观测值 ( z ⃗ k ， R k ) (\vec{z}_k，R_k) (z k，Rk) 的预测值修正
传感器的作用为直接采集系统的状态信息，但其采集的观测值与系统的状态向量，可能存在维度、量纲的差异。为解决该问题，引入观测矩阵 H k H_k Hk ，其功能为将状态向量 x ^ k \hat{x}_k x^k 映射至观测向量 z ⃗ k \vec{z}_k z k 所在的向量空间，实现二者的维度匹配。
基于先验状态估计，可得到观测值的预测分布：
z ⃗ e x p e c t e d = H k x ^ k Σ e x p e c t e d = H k P k H k T \vec z_{expected}=H_k \hat x_k \\ \Sigma_{expected}=H_k P_k H^T_k z expected=Hkx^kΣexpected=HkPkHkT
卡尔曼滤波的优势之一，是对传感器噪声的鲁棒性。所有传感器均存在测量误差，其观测值并非系统的真实状态，而是围绕真实状态的含噪采样值。该类测量误差被建模为零均值的高斯噪声 ，其协方差矩阵记为 R k R_k Rk；传感器的直接测量结果即为该分布的均值，记为 z ⃗ k \vec{z}_k z k。
此时，系统存在两个独立的高斯分布：其一为基于模型的预测分布 ，其二为基于传感器的观测分布。
为得到系统的最优状态估计，需要在预测值与观测值之间找到最优解。其数学逻辑为：两个独立高斯分布的联合概率最大化，等价于两个高斯分布的乘积运算。
6 高斯分布的融合公式推导
首先对一维高斯分布进行推导，概率密度函数定义为：
N ( x ; μ , σ 2 ) = 1 2 π σ 2 exp ( − ( x − μ ) 2 2 σ 2 ) \mathcal{N}(x;\mu,\sigma^2) = \frac{1}{\sqrt{2\pi\sigma^2}} \exp\left(-\frac{(x-\mu)^2}{2\sigma^2}\right) N(x;μ,σ2)=2πσ2 1exp(−2σ2(x−μ)2)
两个独立的一维高斯分布 N ( x ; μ 1 , σ 1 2 ) \mathcal{N}(x;\mu_1,\sigma_1^2) N(x;μ1,σ12) 与 N ( x ; μ 2 , σ 2 2 ) \mathcal{N}(x;\mu_2,\sigma_2^2) N(x;μ2,σ22) 相乘后，经归一化处理得到的新分布仍为高斯分布，其均值与方差满足：
μ n e w = μ 1 + σ 1 2 σ 1 2 + σ 2 2 ( μ 2 − μ 1 ) σ n e w 2 = σ 1 2 σ 2 2 σ 1 2 + σ 2 2 \mu_{new} = \mu_1 + \frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2}(\mu_2-\mu_1) \\ \sigma_{new}^2 = \frac{\sigma_1^2 \sigma_2^2}{\sigma_1^2+\sigma_2^2} μnew=μ1+σ12+σ22σ12(μ2−μ1)σnew2=σ12+σ22σ12σ22
将该系数项定义为卡尔曼增益 K K K：
K = σ 1 2 σ 1 2 + σ 2 2 K = \frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2} K=σ12+σ22σ12
将一维结论推广至多维矩阵形式，若协方差矩阵记为 Σ \Sigma Σ，均值向量记为 μ ⃗ \vec{\mu} μ ，则融合后的均值与协方差为：
μ ⃗ n e w = μ ⃗ 1 + K ( μ ⃗ 2 − H μ ⃗ 1 ) Σ n e w = Σ 1 − K H Σ 1 K = Σ 1 H T ( H Σ 1 H T + Σ 2 ) − 1 \vec{\mu}_{new} = \vec{\mu}_1 + K \left(\vec{\mu}_2 - H\vec{\mu}1\right) \\ \Sigma{new} = \Sigma_1 - K H \Sigma_1 \\ K = \Sigma_1 H^T \left(H\Sigma_1 H^T + \Sigma_2\right)^{-1} μ new=μ 1+K(μ 2−Hμ 1)Σnew=Σ1−KHΣ1K=Σ1HT(HΣ1HT+Σ2)−1
式中，矩阵 K K K 即为卡尔曼增益矩阵，是卡尔曼滤波更新阶段的参数。
7 卡尔曼滤波公式的整合
将预测阶段的先验估计 ( x ^ k , P k ) (\hat{x}_k, P_k) (x^k,Pk) 与观测阶段的测量分布 ( z ⃗ k , R k ) (\vec{z}_k, R_k) (z k,Rk) 代入上述高斯融合公式，其中预测分布的观测映射为 ( H k x ^ k , H k P k H k T ) (H_k\hat{x}_k, H_kP_kH_k^T) (Hkx^k,HkPkHkT)，观测分布为 ( z ⃗ k , R k ) (\vec{z}_k, R_k) (z k,Rk)。
最终推导得到卡尔曼滤波更新阶段的完整公式 ：
K k = P k H k T ( H k P k H k T + R k ) − 1 x ^ k ′ = x ^ k + K k ( z k ⃗ − H k x ^ k ) K_k = P_kH^T_k\left(H_kP_kH^T_k+R_k\right)^{-1} \\ \hat x'_k = \hat x_k+K_k\left(\vec {z_k}-H_k\hat x_k\right) Kk=PkHkT(HkPkHkT+Rk)−1x^k′=x^k+Kk(zk −Hkx^k)
P k ′ = P k − K k H k P k P'_k=P_k-K_kH_kP_k Pk′=Pk−KkHkPk
式中， x ^ k ′ \hat{x}_k' x^k′ 为 k k k 时刻的最优状态估计值 ， P k ′ P_k' Pk′ 为对应的最优协方差矩阵。该最优估计值将作为下一时刻滤波迭代的输入，完成卡尔曼滤波的闭环递推。
参数整定方法
卡尔曼滤波的算法逻辑可通过少量公式完成表达，但滤波效果的优劣，完全取决于参数的合理整定。在航天测控、工业自动化等高精度领域，卡尔曼滤波的参数整定均由专业工程师或专项团队完成，足见该环节的重要性。参数整定的过程兼具理论性与工程实践性，针对同一套数学模型，理论上存在最优参数组合；但实际应用中，不同环境下的过程噪声与观测噪声特性存在差异，仍需通过实测完成参数的迭代优化。参数整定的相关参考可查阅：
- 理解Kalman滤波的使用 - 过客冲冲 - 博客园
https://www.cnblogs.com/jcchen1987/p/4371439.html
卡尔曼滤波中需重点整定的参数为 Q k Q_k Qk、 R k R_k Rk 与初始协方差矩阵 P 0 P_0 P0，各参数的物理意义与整定规律如下：
- 过程噪声协方差矩阵 Q k Q_k Qk
Q k Q_k Qk 量化预测模型的误差水平，其数值越小，代表滤波算法对预测模型的置信度越高，系统的收敛速度越快，同时说明模型的建模精度越高。
- 若 Q k = 0 Q_k = 0 Qk=0，算法完全信任预测值、完全不信任观测值；
- 若 Q k → ∞ Q_k \to \infty Qk→∞，算法完全信任观测值、完全不信任预测值。
- 观测噪声协方差矩阵 R k R_k Rk
R k R_k Rk 量化传感器的测量误差水平，其数值直接决定滤波系统对观测值的置信度。
- 若 R k R_k Rk 过大，系统对观测值的置信度低，滤波结果的响应速度变慢，观测值对状态更新的贡献度小；
- 若 R k R_k Rk 过小，系统对观测值的置信度高，收敛速度加快，但过小的 R k R_k Rk 会导致滤波结果出现震荡，甚至引发滤波器发散。
R k R_k Rk 的取值有两种工程化确定方法：① 对传感器进行批量采样，基于采样数据的正态分布特性，按 3 σ 3\sigma 3σ 原则取 ( 3 σ ) 2 (3\sigma)^2 (3σ)2 作为初始值；② 直接采用传感器厂商提供的标称测量误差。
- 初始协方差矩阵 P 0 P_0 P0
P 0 P_0 P0 表征滤波初始时刻，对状态向量估计值的置信度，其数值仅影响滤波系统的初始收敛速度。
- P 0 P_0 P0 越小，代表对初始状态的置信度越高，初始收敛速度越快；
- 滤波迭代过程中， P k P_k Pk 会随算法自动更新，当系统进入稳态后， P k P_k Pk 将收敛至一个极小的稳定值。
工程化参数整定步骤
对 Q k Q_k Qk 与 R k R_k Rk 的整定，建议遵循先粗调后精调的原则：
- 固定 R k R_k Rk 为较大值，将 Q k Q_k Qk 从极小值逐步增大，观察滤波收敛效果；
- 固定 Q k Q_k Qk 为最优粗调值，将 R k R_k Rk 从极大值逐步减小，优化滤波的响应速度与稳态精度；
- 交替微调 Q k Q_k Qk 与 R k R_k Rk，直至滤波结果兼具快速收敛、无震荡、稳态误差小的特性。
应用 CA 模型
MATLAB 和 Python 例程 1 的观测矩阵仅对位置量进行观测（Python 例程 2 同时对位置与速度进行观测）。本案例的参数配置为： p v a r = 0.1 p_{var}=0.1 pvar=0.1， q v a r = 0.01 q_{var}=0.01 qvar=0.01， σ γ = 0.1 \sigma_{\gamma}=0.1 σγ=0.1。
代码例程（MATLAB）
matlab
clear;
clc;
close all;
%% 构建数据集
dt=0.1;
t=[0:dt:10];
N=length(t);
v_0=10;
a_0=2;
theta=pi/3;
px=v_0*cos(theta)*t+0.5*a_0*t.^2;
py=v_0*sin(theta)*t+0.5*a_0*t.^2;
%% 添加噪声
noise=1.5*randn(1,length(px));%均值为 0 方差为 2.25 的正态分布
noise_px=px+noise;
noise=1.5*randn(1,length(py));%均值为 0 方差为 2.25 的正态分布
noise_py=py+noise;
dataset=[px;py];
noise_dateset=[noise_px;noise_py];
%% 设置卡尔曼滤波矩阵的参数
%CA 模型
X=[noise_dateset(1,1);noise_dateset(2,1);0;0;0;0]; %初始状态 X=[位置 px;位置 py]
p_var=0.1;
P=diag(ones(1,6))*p_var; %状态协方差矩阵
rank=length(P);%矩阵的秩
F=[ 1 0 dt 0 dt^2/2 0;
0 1 0 dt 0 dt^2/2;
0 0 1 0 dt 0;
0 0 0 1 0 dt;
0 0 0 0 1 0;
0 0 0 0 0 1 ]; %状态转移矩阵
H=[1 0 0 0 0 0;
0 1 0 0 0 0];%传感器提供的观测矩阵
r_var=0.1;
R=diag(ones(1,2))*r_var;%传感器的观测噪声协方差矩阵
q_var=0.01;
Q=diag(ones(1,6))*q_var; %状态转移协方差矩阵
%% 卡尔曼滤波
for i = 2:N
X_ = F*X(:,i-1); %基于上一状态预测当前状态 X_为 t 时刻状态预测 (这里没有控制)
P_ = F*P(:,rank*(i-1)-5:rank*(i-1))*F'+Q;%更新协方差 Q 系统过程的协方差
%% 计算卡尔曼增益
K = P_*H'/(H*P_*H'+R);
%% 更新
X(:,i) = X_+K*(noise_dateset(:,i)-H*X_);% 得到当前状态的最优化估算值 增益乘以残差
P(:,rank*i-5:rank*i) = (eye(rank)-K*H)*P_;%更新 K 状态的协方差
end
%% 绘图
figure;
plot(dataset(1,:),dataset(2,:),'r-','LineWidth',1)
hold on;
plot(X(1,:),X(2,:),'g-','LineWidth',1.5);
hold on;
plot(noise_dateset(1,:),noise_dateset(2,:),'b-','LineWidth',1)
xlabel(' 时间/s'),ylabel(' 运动距离 m')
legend('measure','kalman','noise measure')
title('CV 模型卡尔曼滤波 ')
grid on %显示网格 
代码例程 1（Python）
python
#!/usr/bin/python
# -*- coding: utf-8 -*-
'''
this file can single run main function
'''
from numpy.linalg import inv
from numpy import identity
from numpy import matrix, diag, random
import numpy as np
import matplotlib.pyplot as plt
import random
class CVKalmanFilter:
"""
Simple Kalman filter
Control term has been omitted for now
"""
def __init__(self, X, P, F, Q, Z, H, R):
"""
Initialise the filter
Args:
X: State estimate
P: Estimate covariance
F: State transition model
Q: Process noise covariance
Z: Measurement of the state X
H: Observation model
R: Observation noise covariance
"""
self.X = X
self.P = P
self.F = F
self.Q = Q
self.Z = Z
self.H = H
self.R = R
def predict(self, X, P, w=0):
"""
Predict the future state
Args:
X: State estimate
P: Estimate covariance
w: Process noise
Returns:
updated (X, P)
"""
# Project the state ahead
X = np.dot(self.F ,X) + w
P = np.dot(np.dot(self.F , P) , self.F.T) + self.Q
return(X, P)
def update(self, X, P, Z):
"""
Update the Kalman Filter from a measurement
Args:
X: State estimate
P: Estimate covariance
Z: State measurement
Returns:
updated (X, P)
"""
S = np.dot(np.dot(self.H, P), self.H.T) + self.R
K = np.dot(np.dot(P, self.H.T), np.linalg.inv(S))
y = Z - np.dot(self.H, X)
X = X + np.dot(K, y)
P = np.dot((identity(P.shape[1]) - np.dot(K, self.H)), P)
return (X, P)
# Testing
if __name__ == "__main__":
# data set
# own set,The format is dataset=[time px py];
dataset=[]
# Time step size
dt = 0.1
# constuct dataset
t=np.arange(0, 10, dt)
v_0=10
a_0=2
theta=np.pi/3
px=v_0*np.cos(theta)*t+0.5*a_0*t*t
py=v_0*np.sin(theta)*t+0.5*a_0*t*t
# add gauss noise
mu = 0
sigma = 1.5
noise_px=np.zeros(np.array(px).shape)
noise_py=np.zeros(np.array(py).shape)
for i in range(len(px)):
noise_px[i] = px[i] + random.gauss(mu,sigma)
noise_py[i] = py[i] + random.gauss(mu,sigma)
for i in range(len(t)):
c=[]
c.append(t[i])
c.append(noise_px[i])
c.append(noise_py[i])
dataset.append(c)
# Standard deviation of random accelerations
sigma_a = 0.2
# Standard deviation of observations
sigma_z = 0.2
# State vector: [[Position_x],[Position_x] [velocity_x] [velocity_y]
# [acc_x] [acc_y]]
X = np.zeros(6).reshape(6,1)
# State transition model 6*6
F = np.diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
# Initial state covariance 6*6
p_var = 0.1
P = np.diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
P = P * p_var
# Observation vector
Z = np.zeros(2).reshape(2,1)
# Observation model
H = np.array([[ 1., 0., 0., 0., 0., 0.],
[ 0., 1., 0., 0., 0., 0.]])
# Observation covariance
r_var=0.25
R = np.diag([1.0, 1.0])
R = R * r_var
# Process noise covariance matrix
q_var = 0.01
Q = np.diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
Q = Q * q_var
# Initialise the filter
kf = CVKalmanFilter(X, P, F, Q, Z, H, R)
# initial the first value
pre_time=dataset[0][0]
X[0,0]=dataset[0][1]
X[1,0]=dataset[0][2]
# initial result value
kf_result = []
kf_result.append(X)
# Number of measurement_step
measurement_step = len(dataset)
for i in range(1, measurement_step):
t_measurement = dataset[i]
delta_ts = (t_measurement[0] - pre_time)
pre_time = t_measurement[0]
kf.F[0, 2] = delta_ts
kf.F[0, 4] = 0.5 * delta_ts * delta_ts
kf.F[1, 3] = delta_ts
kf.F[1, 5] = 0.5 * delta_ts * delta_ts
kf.F[2, 4] = delta_ts
kf.F[3, 5] = delta_ts
# Predict
(X, P) = kf.predict(X, P)
# Update
m_x = t_measurement[1]
m_y = t_measurement[2]
Z = np.array([[m_x], [m_y]])
(X, P) = kf.update(X, P, Z)
# save the filter position
kf_result.append(X)
# plot
kf_global_x_list = []
kf_global_y_list = []
kf_velocity_x_list = []
kf_velocity_y_list = []
kf_acc_x_list = []
kf_acc_y_list = []
for i in range(len(kf_result)):
kf_global_x_list.append(kf_result[i][0])
kf_global_y_list.append(kf_result[i][1])
kf_velocity_x_list.append(kf_result[i][2])
kf_velocity_y_list.append(kf_result[i][3])
kf_acc_x_list.append(kf_result[i][4])
kf_acc_y_list.append(kf_result[i][5])
plt.figure
plt.plot(px,py, 'r-', label='measure')
plt.plot(noise_px,noise_py, 'b-', label='noise')
plt.plot(kf_global_x_list,kf_global_y_list, 'g-', label='kf estimate')
plt.title("Position")
plt.xlabel("time")
plt.ylabel("d(m)")
plt.legend()
plt.grid(color="k", linestyle=":")
plt.show()
代码例程 2（Python）
本案例的观测矩阵 H H H 为：
H = [ 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 ] H=\begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \end{bmatrix} H= 100001000010000100000000
声明：该代码需要自行完善数据集后即可运行。
python
from numpy.linalg import inv
from numpy import identity
from numpy import matrix, diag, random
import numpy as np
class CVKalmanFilter:
"""
Simple Kalman filter
Control term has been omitted for now
"""
def __init__(self, X, P, F, Q, Z, H, R):
"""
Initialise the filter
Args:
X: State estimate
P: Estimate covariance
F: State transition model
Q: Process noise covariance
Z: Measurement of the state X
H: Observation model
R: Observation noise covariance
"""
self.X = X
self.P = P
self.F = F
self.Q = Q
self.Z = Z
self.H = H
self.R = R
def predict(self, X, P, w=0):
"""
Predict the future state
Args:
X: State estimate
P: Estimate covariance
w: Process noise
Returns:
updated (X, P)
"""
# Project the state ahead
X = np.dot(self.F ,X) + w
P = np.dot(np.dot(self.F , P) , self.F.T) + self.Q
return(X, P)
def update(self, X, P, Z):
"""
Update the Kalman Filter from a measurement
Args:
X: State estimate
P: Estimate covariance
Z: State measurement
Returns:
updated (X, P)
"""
S = np.dot(np.dot(self.H, P), self.H.T) + self.R
K = np.dot(np.dot(P, self.H.T), np.linalg.inv(S))
y = Z - np.dot(self.H, X)
X = X + np.dot(K, y)
P = np.dot((identity(P.shape[1]) - np.dot(K, self.H)), P)
return (X, P)
# Testing
if __name__ == "__main__":
# data set
# own set,The format is dataset=[time px py vx vy ax ay];
dataset=[]
# Standard deviation of random accelerations
sigma_a = 0.2
# Standard deviation of observations
sigma_z = 0.2
# State vector: [[Position_x],[Position_x] [velocity_x] [velocity_y] [acc_x] [acc_y]]
X = np.zeros(6).reshape(6,1)
# State transition model 6*6
F = np.diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
# Initial state covariance 6*6
p_var = 0.1
P = np.diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
P = P * p_var
# Observation vector
Z = np.zeros(4).reshape(4,1)
# Observation model
H = np.array([[ 1., 0., 0., 0., 0., 0.],
[ 0., 1., 0., 0., 0., 0.],
[ 0., 0., 1., 0., 0., 0.],
[ 0., 0., 0., 1., 0., 0.]])
# Observation covariance
r_var=0.25
R = np.diag([1.0, 1.0, 1.0, 1.0])
R = R * r_var
# Process noise covariance matrix
q_var = 0.01
Q = np.diag([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
Q = Q * q_var
# Initialise the filter
kf = CVKalmanFilter(X, P, F, Q, Z, H, R)
# initial the first value
pre_time=dataset[0][0]
X[0,0]=dataset[0][1]
X[1,0]=dataset[0][2]
X[2,0]=dataset[0][3]
X[3,0]=dataset[0][4]
X[4,0]=dataset[0][5]
X[5,0]=dataset[0][6]
# initial result value
kf_result = []
kf_result.append(X)
# Number of measurement_step
measurement_step = len(dataset)
for i in range(1, measurement_step):
t_measurement = dataset[i]
delta_ts = (t_measurement[0] - pre_time)
pre_time = t_measurement[0]
F[0, 2] = delta_ts;
F[0, 4] = 0.5 * delta_ts * delta_ts;
F[1, 3] = delta_ts;
F[1, 5] = 0.5 * delta_ts * delta_ts;
F[2, 4] = delta_ts;
F[3, 5] = delta_ts;
# Predict
(X, P) = kf.predict(X, P)
# Update
m_x = t_measurement[1]
m_y = t_measurement[2]
m_Vx = t_measurement[3]
m_Vy = t_measurement[4]
Z = np.array([[m_x], [m_y],[m_Vx],[m_Vy]])
(X, P) = kf.update(X, P, Z)
kf_result.append(X)应用 CV 模型
恒定速度模型（Constant Velocity, CV）的表达如下：
x ^ k = [ p o s i t i o n v e l o c i t y ] P k = [ Σ p p Σ p v Σ v p Σ v v ] \hat{x}k=\begin{bmatrix} position \\ velocity \end{bmatrix} \quad\quad P_k=\begin{bmatrix} \Sigma{pp} & \Sigma_{pv} \\ \Sigma_{vp} & \Sigma_{vv} \end{bmatrix} x^k=[positionvelocity]Pk=[ΣppΣvpΣpvΣvv]
p k = p k − 1 + Δ t ⋅ v k − 1 p_k=p_{k-1}+\Delta t \cdot v_{k-1} pk=pk−1+Δt⋅vk−1
v k = v k − 1 v_k=v_{k-1} vk=vk−1
x ^ k = [ 1 Δ t 0 1 ] x ^ k − 1 = F k x ^ k − 1 \hat x_k=\begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \hat x_{k-1} = F_k \hat x_{k-1} x^k=[10Δt1]x^k−1=Fkx^k−1
MATLAB 代码
matlab
Z=(1:2:200); %观测值
noise=5*randn(1,100); %均值为 0 方差为 25 的正态分布
moise_Z=Z+noise;%模拟的实际观测值
%CV 模型
X=[moise_Z(1);0]; %初始状态 X=[位置;速度 ]
P=[1 0;0 1]; %状态协方差矩阵
rank=length(P);%矩阵的秩
T=1;
F=[1 T;0 1]; %状态转移矩阵
Q=[0.0001,0;0 , 0.0001]; %状态转移协方差矩阵
H=[1,0];%传感器提供的观测矩阵
R=1;%传感器的观测噪声协方差矩阵
%% 卡尔曼滤波
for i = 2:100
X_ = F*X(:,i-1); %基于上一状态预测当前状态 X_为 t 时刻状态预测 (这里没有控制)
P_ = F*P(:,rank*(i-1)-1:rank*(i-1))*F'+Q;%更新协方差 Q 系统过程的协方差
%% 计算卡尔曼增益
K = P_*H'/(H*P_*H'+R);
%% 更新
X(:,i) = X_+K*(moise_Z(i)-H*X_);% 得到当前状态的最优化估算值 增益乘以残差
P(:,rank*i-1:rank*i) = (eye(rank)-K*H)*P_;%更新 K 状态的协方差
end
%% 绘图
figure;
plot(Z,'r-','LineWidth',1)
hold on;
plot(X(1,:),'g-','LineWidth',1.5);
hold on;
plot(moise_Z,'b-','LineWidth',1)
xlabel(' 时间/s'),ylabel(' 运动距离 m')
legend('measure','kalman','noise measure')
title('CV 模型卡尔曼滤波 ')
grid on %显示网格 
Python 代码
声明：该代码需要自行完善输入数据后即可运行。
python
import numpy as np
import pandas as pd
def kalman_filter(zk, xk, A=np.matrix(1), B=np.matrix(0), Pk=1,
uk=np.array(0), wk=0, Q=0.1, R=1, H=np.matrix(1)):
"""Performs Kalman Filtering on pandas timeseries data.
:param: zk (pandas timeseries): input data
:param: xk (np.array): a priori state estimate vector
:param: A (np.matrix): state transition coefficient matrix
:param: B (np.matrix): control coefficient matrix
:param: Pk (np.matrix): prediction covariance matrix
:param: uk (np.array): control signal vector
:param: wk (float): process noise (has covariance Q)
:param: Q (float): process covariance
:param: R (float): measurement covariance
:param: H (np.matrix): transformation matrix
:return: output (np.array): kalman filtered data
"""
output = np.zeros(len(zk))
for k, t in enumerate(zk.index):
# time update (prediction)
xk = A*xk + B*uk + wk # Predict state
zk_pred = H*xk # Predict measurement
Pk = A*Pk*A.T + Q # Predict error covariance
# measurement update (correction)
vk = zk[t] - zk_pred # Measurement residual
S = (H*Pk*H.T) + R # Prediction covariance
Kk = (Pk*H.T) / S # Compute Kalman Gain
xk = xk + (Kk * vk) # Update estimate with gain * residual
Pk = (1 - Kk*H)*Pk # Update error covariance
output[k] = xk.item()
return output参考链接
扩展卡尔曼滤波（EKF）理论讲解与实例（MATLAB、Python 和 C++代码）
O_MMMM_O 原创于 2020-05-12 16:13:51 发布
本文深度解析扩展卡尔曼滤波 (EKF) 的理论，对比卡尔曼滤波 (KF) 的适用边界，阐述非线性模型（如 CTRV 模型）的工程化应用，并提供 MATLAB、Python 与 C++ 三种语言的完整代码实现，可支撑初学者快速掌握非线性系统的滤波求解方法。
前文介绍的卡尔曼滤波仅适用于线性高斯系统，其状态方程与观测方程均需满足线性关系，且噪声项服从高斯分布。但实际工程中的绝大多数系统均为非线性系统，高斯分布的噪声经非线性变换后，将不再保持高斯特性，此时传统卡尔曼滤波将失效。扩展卡尔曼滤波（EKF）的价值，即为解决非线性高斯系统的状态估计问题。
理论讲解
扩展卡尔曼滤波（Extended Kalman Filter，EKF）的思想为：通过局部线性化近似，将非线性系统转化为线性系统求解 。具体方法为对非线性的状态方程与观测方程，在当前状态估计值的均值处进行一阶泰勒展开，舍弃高阶余项，仅保留一阶线性项，实现非线性系统的局部线性化近似，之后即可沿用传统卡尔曼滤波的算法框架完成求解。
泰勒展开的数学基础
数学中，泰勒公式的作用是利用函数在某一点的局部信息，对其邻域内的函数值进行高精度逼近，其本质为用多项式函数拟合光滑的非线性函数。若函数 f ( x ) f(x) f(x) 足够光滑，则其泰勒展开式为：
f ( x ) T a y l o r = ∑ n = 0 ∞ f ( n ) ( a ) n ! × ( x − a ) n = f ( a ) + f ′ ( a ) 1 ! ( x − a ) + f ( 2 ) ( a ) 2 ! ( x − a ) 2 + ⋯ + f ( n ) ( a ) n ! ( x − a ) n + R n ( x ) f(x){Taylor} = \sum{n=0}^{\infty} \frac{f^{(n)}(a)}{n!} \times (x - a)^n =f(a) + \frac{f'(a)}{1!}(x-a) + \frac{f^{(2)}(a)}{2!}(x-a)^2+ \cdots + \frac{f^{(n)}(a)}{n!}(x-a)^n + R_n(x) f(x)Taylor=n=0∑∞n!f(n)(a)×(x−a)n=f(a)+1!f′(a)(x−a)+2!f(2)(a)(x−a)2+⋯+n!f(n)(a)(x−a)n+Rn(x)
式中：
- f ( n ) ( a ) f^{(n)}(a) f(n)(a) 为函数 f ( x ) f(x) f(x) 在点 a a a 处的 n n n 阶导数；
- 1 n ! \frac{1}{n!} n!1 为各阶项的系数；
- ( x − a ) n (x-a)^n (x−a)n 为自变量的幂次项；
- R n ( x ) R_n(x) Rn(x) 为泰勒展开的余项，是 ( x − a ) n (x-a)^n (x−a)n 的高阶无穷小。
EKF 中仅采用一阶泰勒展开 ，即仅保留常数项与一阶线性项，余项被忽略，这也是 EKF 被称为一阶滤波算法的原因。
KF 与 EKF 模型对比
模型的数学表达
卡尔曼滤波与扩展卡尔曼滤波的区别，在于状态方程与观测方程的线性性差异，二者的模型对比如下：
系统模型
Kalman Filter Extended Kalman Filter \qquad \textbf{Kalman Filter} \qquad \qquad \qquad \qquad \textbf{Extended Kalman Filter} Kalman FilterExtended Kalman Filter
x k = F x k − 1 + B u k + w k x k = f ( x k − 1 , u k ) + w k z ⃗ k = H x k + v k z ⃗ k = h ( x k ) + v k x_k=F x_{k-1}+B u_k+w_k \qquad \qquad x_k=f(x_{k-1},u_k)+w_k \\ \vec{z}_k=H x_k+v_k \qquad \qquad \qquad \vec{z}_k=h(x_k)+v_k xk=Fxk−1+Buk+wkxk=f(xk−1,uk)+wkz k=Hxk+vkz k=h(xk)+vk
预测公式
Kalman Filter Extended Kalman Filter \qquad \textbf{Kalman Filter} \qquad \qquad \qquad \qquad \textbf{Extended Kalman Filter} Kalman FilterExtended Kalman Filter
x ^ k = F k x ^ k − 1 + B k u ⃗ k x ^ k = f ( x ^ k − 1 , u k ) P k = F k P k − 1 F k T + Q k P k = F k P k − 1 F k T + Q k \hat x_k=F_k \hat x_{k-1} + B_k \vec u_k \qquad \qquad \hat x_k=f(\hat x_{k-1},u_k) \\ P_k=F_kP_{k-1}F^T_k+Q_k \qquad \qquad P_k=F_kP_{k-1}F^T_k+Q_k x^k=Fkx^k−1+Bku kx^k=f(x^k−1,uk)Pk=FkPk−1FkT+QkPk=FkPk−1FkT+Qk
更新公式
Kalman Filter Extended Kalman Filter \qquad \textbf{Kalman Filter} \qquad \qquad \qquad \qquad \textbf{Extended Kalman Filter} Kalman FilterExtended Kalman Filter
K k = P k H k T ( H k P k H k T + R k ) − 1 K k = P k H k T ( H k P k H k T + R k ) − 1 x ^ k ′ = x ^ k + K k ( z ⃗ k − H k x ^ k ) x ^ k ′ = x ^ k + K k ( z ⃗ k − h ( x ^ k ) ) P k ′ = P k − K k H k P k P k ′ = P k − K k H k P k K_k = P_kH^T_k\left(H_kP_kH^T_k+R_k\right)^{-1} \qquad K_k = P_kH^T_k\left(H_kP_kH^T_k+R_k\right)^{-1} \\ \hat x'_k = \hat x_k+K_k\left(\vec z_k-H_k\hat x_k\right) \qquad \hat x'_k = \hat x_k+K_k\left(\vec z_k-h(\hat x_k)\right) \\ P'_k=P_k-K_kH_kP_k \qquad \qquad P'_k=P_k-K_kH_kP_k Kk=PkHkT(HkPkHkT+Rk)−1Kk=PkHkT(HkPkHkT+Rk)−1x^k′=x^k+Kk(z k−Hkx^k)x^k′=x^k+Kk(z k−h(x^k))Pk′=Pk−KkHkPkPk′=Pk−KkHkPk
差异总结
- 算法结构一致 ：KF 与 EKF 均基于高斯分布描述系统的后验概率密度，均通过贝叶斯递推公式完成状态估计，滤波流程均分为预测阶段 与更新阶段；
- 区别 ：EKF 中的状态转移矩阵 F k F_k Fk 与观测矩阵 H k H_k Hk，不再是固定的线性矩阵，而是由非线性函数的雅克比矩阵 替代：
- F k F_k Fk 为非线性状态函数 f ( ⋅ ) f(\cdot) f(⋅) 在 x ^ k − 1 \hat{x}_{k-1} x^k−1 处的雅克比矩阵；
- H k H_k Hk 为非线性观测函数 h ( ⋅ ) h(\cdot) h(⋅) 在 x ^ k \hat{x}_k x^k 处的雅克比矩阵。
其中
x ^ k \hat{x}_k x^k ： k k k 时刻的系统状态向量；
F k F_k Fk ：非线性状态函数的雅克比矩阵，实现非线性状态的局部线性化映射；
B k B_k Bk ：输入控制矩阵，适用于带外部控制的非线性系统；
u ⃗ k \vec{u}_k u k ：系统的控制向量；
w k w_k wk ：过程噪声，服从零均值高斯分布；
v k v_k vk ：观测噪声，服从零均值高斯分布；
P k P_k Pk ：状态向量的协方差矩阵；
Q k Q_k Qk ：过程噪声的协方差矩阵；
z ⃗ k \vec{z}_k z k ：传感器的观测向量；
H k H_k Hk ：非线性观测函数的雅克比矩阵；
R k R_k Rk ：观测噪声的协方差矩阵。
J A 、 J H J_A、J_H JA、JH 均为雅克比矩阵。
雅克比矩阵计算
雅克比矩阵的定义
雅克比矩阵（Jacobian Matrix）是表征非线性函数局部线性特性的矩阵，其本质为非线性函数的所有分量对状态向量所有分量的一阶偏导数构成的矩阵。对于非线性函数，雅克比矩阵是一阶泰勒展开的系数矩阵，也是实现非线性系统线性化的工具。
状态雅克比矩阵 F k F_k Fk 的推导
非线性状态方程为 x k = f ( x k − 1 , u k , w k ) x_k=f(x_{k-1},u_k,w_k) xk=f(xk−1,uk,wk)，在均值点 x ^ k − 1 \hat{x}{k-1} x^k−1 处进行一阶泰勒展开（忽略过程噪声 w k = 0 w_k=0 wk=0，余项忽略）：
x k ≈ f ( x ^ k − 1 , u k , 0 ) + ∂ f ∂ x k − 1 ∣ x ^ k − 1 , u k , 0 ( x k − 1 − x ^ k − 1 ) x_k \approx f(\hat x{k-1},u_k,0)+ \left.\frac{\partial {f}}{\partial {x}{k-1}}\right|{\hat{x}{k-1},u_k,0} \left(x{k-1}-\hat{x}_{k-1}\right) xk≈f(x^k−1,uk,0)+∂xk−1∂f x^k−1,uk,0(xk−1−x^k−1)
定义状态雅克比矩阵 F k F_k Fk 为：
F k = d e f ∂ f ( x ) ∂ x = [ ∂ f 1 ∂ x 1 ⋯ ∂ f 1 ∂ x n ⋮ ⋱ ⋮ ∂ f m ∂ x 1 ⋯ ∂ f m ∂ x n ] = d e f J A F_k \stackrel{def}{=} \frac{\partial f(x)}{\partial x} = \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \cdots & \frac{\partial f_1}{\partial x_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial f_m}{\partial x_1} & \cdots & \frac{\partial f_m}{\partial x_n} \end{bmatrix} \stackrel{def}{=} J_A Fk=def∂x∂f(x)= ∂x1∂f1⋮∂x1∂fm⋯⋱⋯∂xn∂f1⋮∂xn∂fm =defJA
式中， f ( x ) f(x) f(x) 为 m m m 维非线性状态函数， x x x 为 n n n 维状态向量， F k F_k Fk 为 m × n m \times n m×n 阶矩阵。
观测雅克比矩阵 H k H_k Hk 的推导
非线性观测方程为 z ⃗ k = h ( x k , v k ) \vec{z}k=h(x_k,v_k) z k=h(xk,vk)，在均值点 x ^ k \hat{x}k x^k 处进行一阶泰勒展开（忽略观测噪声 v k = 0 v_k=0 vk=0，余项忽略）：
z ⃗ k ≈ h ( x ^ k , 0 ) + ∂ h ∂ x k ∣ x ^ k , 0 ( x k − x ^ k ) \vec{z}k \approx h(\hat x_k,0)+ \left.\frac{\partial {h}}{\partial {x}{k}}\right|{\hat{x}{k},0} \left(x_{k}-\hat{x}_{k}\right) z k≈h(x^k,0)+∂xk∂h x^k,0(xk−x^k)
定义观测雅克比矩阵 H k H_k Hk 为：
H k = d e f ∂ h ( x ) ∂ x = [ ∂ h 1 ∂ x 1 ⋯ ∂ h 1 ∂ x n ⋮ ⋱ ⋮ ∂ h m ∂ x 1 ⋯ ∂ h m ∂ x n ] = d e f J H H_k \stackrel{def}{=} \frac{\partial h(x)}{\partial x} = \begin{bmatrix} \frac{\partial h_1}{\partial x_1} & \cdots & \frac{\partial h_1}{\partial x_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial h_m}{\partial x_1} & \cdots & \frac{\partial h_m}{\partial x_n} \end{bmatrix} \stackrel{def}{=} J_H Hk=def∂x∂h(x)= ∂x1∂h1⋮∂x1∂hm⋯⋱⋯∂xn∂h1⋮∂xn∂hm =defJH
式中， h ( x ) h(x) h(x) 为 m m m 维非线性观测函数， x x x 为 n n n 维状态向量， H k H_k Hk 为 m × n m \times n m×n 阶矩阵。
雅克比矩阵计算实例
已知二维非线性状态函数：
f ( x ) = [ f 1 f 2 ] = [ x 1 + x 2 x 1 2 ] {f}({x})=\begin{bmatrix} f_1 \\ f_2 \end{bmatrix}=\begin{bmatrix} x_1+x_2 \\ x_1^2 \end{bmatrix} f(x)=[f1f2]=[x1+x2x12]
其状态向量为 x = [ x 1 x 2 ] T x=\begin{bmatrix}x_1 & x_2\end{bmatrix}^T x=[x1x2]T，则雅克比矩阵为：
∂ f ∂ x = [ ∂ f 1 ∂ x 1 ∂ f 1 ∂ x 2 ∂ f 2 ∂ x 1 ∂ f 2 ∂ x 2 ] = [ 1 1 2 x 1 0 ] \frac{\partial {f}}{\partial {x}}=\begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2} \\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} \end{bmatrix} = \begin{bmatrix} 1 & 1 \\ 2x_1 & 0 \end{bmatrix} ∂x∂f=[∂x1∂f1∂x1∂f2∂x2∂f1∂x2∂f2]=[12x110]
又如工程中常见的极坐标-直角坐标转换函数，其雅克比矩阵计算如下：
应用实例
线性模型
工程中常见的线性运动模型，均适用于传统卡尔曼滤波，该类模型的特征为状态方程与观测方程均为线性，无转弯、变加速等非线性行为。主要包含两类基础模型：
- 恒定速度模型（Constant Velocity, CV）：适用于匀速直线运动的系统；
- 恒定加速度模型（Constant Acceleration, CA）：适用于匀加速直线运动的系统。
CV 模型
CV 模型的状态向量定义为：
x → ( t ) = ( x y v x v y ) T \overrightarrow{x}(t) = \left( x \quad y \quad v_x \quad v_y \right)^T x (t)=(xyvxvy)T
状态转移方程为：
x → ( t + Δ t ) = [ x ( t ) + Δ t ⋅ v x y ( t ) + Δ t ⋅ v y v x v y ] \overrightarrow{x}(t+\Delta t) = \begin{bmatrix} x(t)+\Delta t \cdot v_x \\ y(t)+\Delta t \cdot v_y \\ v_x \\ v_y \end{bmatrix} x (t+Δt)= x(t)+Δt⋅vxy(t)+Δt⋅vyvxvy
其扩展形式可包含加速度项，状态向量为：
x → ( t ) = ( x y v x v y a x a y ) T \overrightarrow{x}(t) = \left( x \quad y \quad v_x \quad v_y \quad a_x \quad a_y \right)^T x (t)=(xyvxvyaxay)T
矩阵形式的状态转移方程为：
x 1 → = F ⋅ x 0 → = [ 1 0 t 0 0 0 0 1 0 t 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 ] ⋅ x 0 → \overrightarrow{x_1} = F \cdot \overrightarrow{x_0}= \begin{bmatrix} 1 & 0 & t & 0 & 0 & 0 \\ 0 & 1 & 0 & t & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \cdot \overrightarrow{x_0} x1 =F⋅x0 = 100000010000t010000t0100000010000001 ⋅x0
CA 模型
CA 模型的状态向量定义为：
x → ( t ) = ( x y v x v y a x a y ) T \overrightarrow{x}(t) = \left( x \quad y \quad v_x \quad v_y \quad a_x \quad a_y \right)^T x (t)=(xyvxvyaxay)T
矩阵形式的状态转移方程为：
x 1 → = F ⋅ x 0 → = [ 1 0 t 0 t 2 2 0 0 1 0 t 0 t 2 2 0 0 1 0 t 0 0 0 0 1 0 t 0 0 0 0 1 0 0 0 0 0 0 1 ] ⋅ x 0 → \overrightarrow{x_1} = F \cdot \overrightarrow{x_0}= \begin{bmatrix} 1 & 0 & t & 0 & \frac{t^2}{2} & 0 \\ 0 & 1 & 0 & t & 0 & \frac{t^2}{2} \\ 0 & 0 & 1 & 0 & t & 0 \\ 0 & 0 & 0 & 1 & 0 & t \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \cdot \overrightarrow{x_0} x1 =F⋅x0 = 100000010000t010000t01002t20t01002t20t01 ⋅x0
CA 与 CV 模型的交互式多模型（IMM），可参考我的博客：交互式多模型 IMM 的原理及代码实现（MATLAB）
非线性模型
实际工程中的系统普遍存在非线性行为（如转弯、变向、变速等），此时需采用非线性运动模型，主流的非线性模型包含：
- 恒定转率和速度模型（Constant Turn Rate and Velocity，CTRV）：适用于带转弯的匀速运动系统；
- 恒定转率和加速度模型（Constant Turn Rate and Acceleration，CTRA）：适用于带转弯的匀加速运动系统。
CTRV 模型是目前车载、机载目标追踪领域的主流模型，该模型假设目标的速度大小恒定 、偏航角速度恒定，可精准表征目标的转弯行为。其局限性为：偏航角速度的测量扰动会导致模型精度下降，即使目标静止，角速度也会存在微小波动。为解决该问题，衍生出恒定转向角和速度模型（CSAV）、常曲率和加速度模型（CCA）等改进模型。
CTRV 模型
CV、CA 模型无法表征目标的转弯行为，CTRV 模型的价值即为解决带转弯运动的状态估计问题。
CTRV 模型的状态向量定义为：
x → ( t ) = ( p x p y v ψ ψ ˙ ) T \overrightarrow{x}(t) = \left( p_x \quad p_y \quad v \quad \psi \quad \dot{\psi} \right)^T x (t)=(pxpyvψψ˙)T
式中：
- p x , p y p_x, p_y px,py 为目标的位置坐标；
- v v v 为目标的速度大小（恒定）；
- ψ \psi ψ 为目标的偏航角（与 x 轴的夹角，逆时针为正，取值范围 [ 0 , 2 π ) [0,2\pi) [0,2π)）；
- ψ ˙ \dot{\psi} ψ˙ 为目标的偏航角速度（恒定）。
其状态转移的非线性推导过程如下：
CTRV 模型的状态向量也可记为：
x → ( t ) = ( x y v θ ω ) T \overrightarrow{x}(t) = \left( x \quad y \quad v \quad \theta \quad \omega \right)^T x (t)=(xyvθω)T
式中， θ \theta θ 为偏航角， ω \omega ω 为偏航角速度。对应的非线性状态转移函数为：
x → ( t + Δ t ) = [ v ω sin ( ω Δ t + θ ) − v ω sin ( θ ) + x ( t ) − v ω cos ( ω Δ t + θ ) + v ω cos ( θ ) + y ( t ) v ω Δ t + θ ω ] \overrightarrow{x}(t+\Delta t) = \begin{bmatrix} \frac{v}{\omega}\sin(\omega\Delta t+\theta)-\frac{v}{\omega}\sin(\theta)+x(t) \\ -\frac{v}{\omega}\cos(\omega\Delta t+\theta)+\frac{v}{\omega}\cos(\theta)+y(t) \\ v \\ \omega\Delta t+\theta \\ \omega \end{bmatrix} x (t+Δt)= ωvsin(ωΔt+θ)−ωvsin(θ)+x(t)−ωvcos(ωΔt+θ)+ωvcos(θ)+y(t)vωΔt+θω
特殊情况处理： ω = 0 \omega=0 ω=0
当偏航角速度 ω = 0 \omega=0 ω=0 时，上述状态转移函数将出现分母为零的奇点，此时目标为匀速直线运动 ，状态转移函数退化为线性形式：
x ( t + Δ t ) = v cos ( θ ) Δ t + x ( t ) x(t+\Delta t) = v \cos(\theta)\Delta t + x(t) x(t+Δt)=vcos(θ)Δt+x(t)
y ( t + Δ t ) = v sin ( θ ) Δ t + y ( t ) y(t+\Delta t) = v \sin(\theta)\Delta t + y(t) y(t+Δt)=vsin(θ)Δt+y(t)
CTRV 模型为典型的非线性模型，传统卡尔曼滤波无法直接求解，必须通过扩展卡尔曼滤波完成状态的线性化近似与最优估计。
CTRV 模型工程应用示例
应用场景
本案例为多传感器融合的目标追踪场景：采用激光雷达与毫米波雷达，对运动目标的状态进行联合估计，两类传感器的测量特性如下：
- 激光雷达 ：直接测量目标的直角坐标 ( x , y ) (x,y) (x,y)，以本车为坐标原点，车头为 x 轴，车身左侧为 y 轴；测量模型为线性模型。
- 毫米波雷达 ：测量目标的极坐标信息，包括：目标与本车的距离 ρ \rho ρ、目标的方位角 ψ \psi ψ、目标的径向速度 ρ ˙ \dot{\rho} ρ˙（速度在目标-本车连线上的分量）；测量模型为非线性模型。
观测模型的线性/非线性区分
- 激光雷达的线性观测模型
观测矩阵 H L H_L HL 为线性矩阵，实现状态向量到观测向量的直接映射：
H L = [ 1 0 0 0 0 0 1 0 0 0 ] H_L = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \end{bmatrix} HL=[1001000000]
观测映射关系为： H L x → = ( x , y ) T H_L\overrightarrow{x} = (x, y)^T HLx =(x,y)T。
- 毫米波雷达的非线性观测模型
极坐标与直角坐标的转换为非线性关系，观测函数定义为：
ρ ψ ρ ˙ \] = \[ x 2 + y 2 arctan 2 ( y , x ) v x ⋅ x + v y ⋅ y x 2 + y 2 \] = h ( x ) \\begin{bmatrix} \\rho \\\\ \\psi \\\\ \\dot{\\rho} \\end{bmatrix} = \\begin{bmatrix} \\sqrt{x\^2+y\^2} \\\\ \\arctan2(y,x) \\\\ \\frac{vx \\cdot x + vy \\cdot y}{\\sqrt{x\^2+y\^2}} \\end{bmatrix} = h(x) ρψρ˙ = x2+y2 arctan2(y,x)x2+y2 vx⋅x+vy⋅y =h(x) 该非线性函数的雅克比矩阵 H k H_k Hk 为 EKF 的计算项，具体推导与实现见工程代码。  ##### 滤波效果  > 高阶运动模型与扩展卡尔曼滤波的详细推导，可参考：[高级运动模型和扩展卡尔曼滤波](https://blog.csdn.net/AdamShan/article/details/78265754) #### 工程代码链接 1. Python 完整代码：[python_EKF_CTRV 代码](https://download.csdn.net/download/O_MMMM_O/12412228) 2. C++ 完整代码：[C++_EKF_CTRV 代码](https://download.csdn.net/download/O_MMMM_O/12412212) ### MATLAB 示例：抛物线轨迹滤波 #### 场景描述 本案例为基于 EKF 的抛物线轨迹估计，目标做带阻尼的抛体运动，观测值为极坐标的距离与角度，含高斯噪声，滤波目标为估计目标的位置与速度。   #### MATLAB 代码 ```matlab close all; clear all; %% 真实轨迹模拟 kx = .01; ky = .05; % 阻尼系数 g = 9.8; % 重力 t = 15; % 仿真时间 Ts = 0.1; % 采样周期 len = fix(t/Ts); % 仿真步数 dax = 3; day = 3; % 系统噪声 X = zeros(len,4); X(1,:) = [0, 50, 500, 0]; % 状态模拟的初值 for k=2:len x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4); x = x + vx*Ts; vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts; y = y + vy*Ts; vy = vy + (ky*vy^2-g+day*randn(1))*Ts; X(k,:) = [x, vx, y, vy]; end %% 构造量测量 dr = 8; dafa = 0.1; % 量测噪声 for k=1:len r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1); a = atan(X(k,1)/X(k,3))*57.3 + dafa*randn(1,1); Z(k,:) = [r, a]; end %% EKF 滤波 Qk = diag([0; dax/10; 0; day/10])^2; Rk = diag([dr; dafa])^2; Pk = 10*eye(4); Pkk_1 = 10*eye(4); x_hat = [0,40,400,0]'; X_est = zeros(len,4); x_forecast = zeros(4,1); z = zeros(4,1); for k=1:len % 1 状态预测 x1 = x_hat(1) + x_hat(2)*Ts; vx1 = x_hat(2) + (-kx*x_hat(2)^2)*Ts; y1 = x_hat(3) + x_hat(4)*Ts; vy1 = x_hat(4) + (ky*x_hat(4)^2-g)*Ts; x_forecast = [x1; vx1; y1; vy1]; %预测值 % 2 观测预测 r = sqrt(x1*x1+y1*y1); alpha = atan(x1/y1)*57.3; y_yuce = [r,alpha]'; % 状态雅克比矩阵 vx = x_forecast(2); vy = x_forecast(4); F = zeros(4,4); F(1,1) = 1; F(1,2) = Ts; F(2,2) = 1-2*kx*vx*Ts; F(3,3) = 1; F(3,4) = Ts; F(4,4) = 1+2*ky*vy*Ts; Pkk_1 = F*Pk*F'+Qk; % 观测雅克比矩阵 x = x_forecast(1); y = x_forecast(3); H = zeros(2,4); r = sqrt(x^2+y^2); xy2 = 1+(x/y)^2; H(1,1) = x/r; H(1,3) = y/r; H(2,1) = (1/y)/xy2; H(2,3) = (-x/y^2)/xy2; Kk = Pkk_1*H'*(H*Pkk_1*H'+Rk)^-1; %计算增益 x_hat = x_forecast+Kk*(Z(k,:)'-y_yuce); %状态校正 Pk = (eye(4)-Kk*H)*Pkk_1; %协方差校正 X_est(k,:) = x_hat; end %% 绘图展示 figure, hold on, grid on; plot(X(:,1),X(:,3),'-b'); plot(Z(:,1).*sin(Z(:,2)*pi/180), Z(:,1).*cos(Z(:,2)*pi/180)); plot(X_est(:,1),X_est(:,3), 'r'); xlabel('X'); ylabel('Y'); title('EKF simulation'); legend('real', 'measurement', 'ekf estimated'); axis([-5,230,290,530]); ``` ##### 滤波效果图示  > 优化后的完整 MATLAB 代码（收敛速度更快）：[完整的 MATLAB 代码](https://download.csdn.net/download/O_MMMM_O/12412471) > > 优化后效果： ### C++ 示例 #### 示例 1：飞机高度估计 ##### 场景描述 飞机以恒定水平速度飞行，高度保持不变；地面雷达测量飞机与雷达的直线距离 r r r，滤波目标为估计飞机的水平位置 x x x、水平速度 x ˙ \\dot{x} x˙ 与垂直高度 y y y。几何关系为： θ = arctan ( y x ) \\theta=\\arctan\\left(\\frac{y}{x}\\right) θ=arctan(xy) r 2 = x 2 + y 2 r\^2=x\^2+y\^2 r2=x2+y2 状态向量定义为： x = \[ d i s t a n c e v e l o c i t y a l t i t u d e \] = \[ x x ˙ y \] \\mathbf{x}=\\begin{bmatrix} distance \\\\ velocity \\\\ altitude \\end{bmatrix}=\\begin{bmatrix} x \\\\ \\dot{x} \\\\ y \\end{bmatrix} x= distancevelocityaltitude = xx˙y 非线性观测函数为： h ( x \^ ) = x 2 + y 2 h\\left(\\hat{x}\\right)=\\sqrt{x\^2+y\^2} h(x\^)=x2+y2 ，其雅克比矩阵为： J H = \[ x x 2 + y 2 0 y x 2 + y 2 \] J_H=\\left\[\\frac{x}{\\sqrt{x\^2+y\^2}} \\quad 0 \\quad \\frac{y}{\\sqrt{x\^2+y\^2}}\\right\] JH=\[x2+y2 x0x2+y2 y
状态转移的雅克比矩阵为：
J A = [ 1 Δ t 0 0 1 0 0 0 1 ] J_A=\begin{bmatrix} 1 & \Delta t & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} JA= 100Δt10001
代码链接
示例 2：飞机二维运动估计
场景描述
飞机在二维空间内的运动受推力、阻力、升力与重力的联合作用，其运动方程为非线性微分方程，滤波目标为估计飞机的位置、速度与加速度。系统参数为：
- m m m ：飞机质量（1000 kg）；
- b x b_x bx ：阻力系数（0.35 N/(m²·s²)）；
- p p p ：升力系数（3.92 N/(m²·s²)）；
- g g g ：重力加速度（9.8 m/s²）；
- u u u ：电机推力（外部控制量）。
运动方程为非线性连续方程，其雅克比矩阵的推导如下：
代码链接
EKF 的不足
扩展卡尔曼滤波作为一阶非线性滤波算法，其优势为实现简单、计算量小，是工程中最常用的非线性滤波算法，但同时存在显著的局限性，主要体现在以下两点：
1 线性化误差导致的滤波精度损失与发散风险
EKF 是对非线性函数进行一阶泰勒展开 ，该近似方法的本质是舍弃了高阶余项，不可避免地引入线性化误差 。该误差的大小与非线性函数的曲率正相关：函数的非线性越强，线性化误差越大，滤波精度越低；当误差累积到一定程度时，将导致协方差矩阵的非正定，最终引发滤波器发散。
线性化误差的工程补偿方法
针对该问题，工程中常用三类补偿策略，其逻辑均为增大卡尔曼增益，降低预测值的权重、提升观测值的权重，抵消线性化误差的影响：
A) 增大过程噪声协方差矩阵 Q k Q_k Qk，即引入"人为噪声"补偿预测误差；
B) 对预测协方差矩阵 P k P_k Pk 乘以标量加权因子 φ > 1 \varphi>1 φ>1，扩大预测的不确定性；
C) 对预测协方差矩阵 P k P_k Pk 乘以对角加权矩阵 φ = d i a g ( φ i ) \varphi=diag(\sqrt{\varphi_i}) φ=diag(φi )， φ i > 1 \varphi_i>1 φi>1，针对性扩大各状态量的不确定性。
2 雅克比矩阵的计算复杂度高、工程实现难度大
EKF 的计算项为非线性函数的雅克比矩阵，对于简单的非线性函数，雅克比矩阵的推导与编程实现较为容易；但对于复杂的工程系统（如多自由度机械臂、无人机姿态估计），非线性函数的偏导数推导极为繁琐，甚至无法得到解析解。
高阶扩展卡尔曼滤波（如二阶 EKF）可通过引入海塞矩阵降低线性化误差，滤波精度优于一阶 EKF，但海塞矩阵的计算量呈指数级增长，工程实用性极低；三阶及以上的 EKF 无明显精度提升，且计算量过大，几乎无工程应用价值。
为解决 EKF 的上述局限性，可采用无迹卡尔曼滤波（UKF），相关理论与实现可参见：无损卡尔曼滤波
参考文献
无迹（损）卡尔曼滤波（UKF）理论讲解与实例
O_MMMM_O 原创于 2020-05-15 14:23:10 发布
理论讲解
前两篇博客的卡尔曼滤波和扩展卡尔曼滤波均将问题转化为线性高斯模型，因此可直接求解贝叶斯递推公式的解析形式，便于运算。针对非线性问题，扩展卡尔曼滤波不仅存在计算量偏大的问题，还会引入线性化误差，因此亟需一种更优的非线性滤波方法。
EKF 基于高斯分布假设，通过泰勒展开将非线性模型线性化，进而求解预测模型的概率分布（均值与方差）；而无迹卡尔曼滤波（Unscented Kalman Filter，UKF）则通过无迹变换（Unscented Transform，UT）完成预测模型均值与方差的求解，二者的思路对比如下。
EKF 通过泰勒展开对模型完成线性化，进而求解预测模型的均值与方差 \color{darkorange}{\textbf{EKF 通过泰勒展开对模型完成线性化，进而求解预测模型的均值与方差}} EKF 通过泰勒展开对模型完成线性化，进而求解预测模型的均值与方差
UKF 通过无迹变换，直接求解预测模型的均值与方差 \color{darkorange}{\textbf{UKF 通过无迹变换，直接求解预测模型的均值与方差}} UKF 通过无迹变换，直接求解预测模型的均值与方差
UKF 通过构造若干采样点对非线性分布进行近似，由该组采样点确定状态量 x x x与协方差矩阵 P P P的有效取值范围。该思路与粒子滤波存在相似性，但区别为：UKF 中 Sigma 点的生成过程无概率随机性参与。UKF 的本质是将单一非线性变量的不确定性，通过多个 Sigma 点的整体分布特征完成近似表征。
模型对比
| 模型 | 缺点 | UKF 对应的改进策略 |
|---|---|---|
| KF | 仅适用于线性高斯系统的状态估计 | 可适配非线性系统的状态估计需求 |
| EKF | 泰勒一阶展开的线性化处理会舍弃高阶项，在强非线性系统中引入显著误差；线性化过程需计算雅克比（Jacobian）矩阵，提升运算复杂度 | 对非线性系统的概率密度分布直接近似，无高阶项舍弃带来的误差；无需计算雅克比矩阵，大幅降低运算复杂度 |
UT 变换
无迹变换（Unscented Transform, UT）是一种对非线性随机变量各阶矩进行近似求解的数值方法，能够高效处理非线性系统的状态传递问题。该方法通过既定规则的采样与加权运算，对非线性变换后的随机变量均值与方差完成高精度近似，基于无迹变换实现的 UKF，其对统计矩的近似精度可达到二阶 EKF 的水平。
无迹变换的数学定义如下：
设存在非线性映射关系 y = f ( x ) \boldsymbol{y} = \boldsymbol{f}(\boldsymbol{x}) y=f(x)，其中 x \boldsymbol{x} x为 n n n维的状态随机向量，且已知该向量的均值为 x ‾ \overline{\boldsymbol{x}} x，协方差矩阵为 P x \boldsymbol{P}_x Px。
通过无迹变换可构造出 2 n + 1 2n+1 2n+1个 Sigma 采样点 x i \boldsymbol{x}_i xi，并为每个采样点分配对应的权值 W i W_i Wi；将所有 Sigma 点代入非线性函数 f ( ⋅ ) \boldsymbol{f}(\cdot) f(⋅)后，通过加权求和即可得到输出量 y \boldsymbol{y} y的统计特征（均值与方差）。
该过程等价于在概率论中，已知随机变量 x \boldsymbol{x} x的数字特征，求解其非线性映射 y \boldsymbol{y} y数字特征的过程。
UKF 算法步骤
UKF 的完整递推流程可分为初始化、Sigma 点生成、预测、量测映射、状态更新共 6 个步骤，具体如下：
- 完成系统初始状态的赋值： x k , P k \boldsymbol{x}_k, \boldsymbol{P}_k xk,Pk；
- 基于当前时刻的状态均值 x k \boldsymbol{x}_k xk与协方差 P k \boldsymbol{P}_k Pk，生成对应维度的 Sigma 点集 X k \boldsymbol{X}_k Xk；
- 将 Sigma 点集代入系统的状态转移模型，完成一步预测得到预测 Sigma 点集 X k + 1 ∣ k \boldsymbol{X}_{k+1|k} Xk+1∣k；
- 对预测 Sigma 点集加权求和，得到状态的先验均值 x k + 1 ∣ k \boldsymbol{x}{k+1|k} xk+1∣k与先验协方差 P k + 1 ∣ k \boldsymbol{P}{k+1|k} Pk+1∣k；
- 当获得 k + 1 k+1 k+1时刻的量测值后，将预测 Sigma 点集代入量测模型，映射至量测空间得到预测量测点集 Z k + 1 ∣ k \boldsymbol{Z}_{k+1|k} Zk+1∣k；
- 利用预测量测与实际量测的残差，完成状态的后验更新，得到 x k + 1 ∣ k + 1 , P k + 1 ∣ k + 1 \boldsymbol{x}{k+1|k+1}, \boldsymbol{P}{k+1|k+1} xk+1∣k+1,Pk+1∣k+1（本质为状态先验分布与量测分布的高斯乘积，得到后验高斯分布）。
预测部分
UKF 的预测环节本质是无迹变换在状态空间的具体应用，是通过 Sigma 点完成非线性状态转移的分布近似，具体实现流程分为两个阶段。
1. Sigma 点集的生成
基于 k k k时刻的状态后验均值 x k ∣ k \boldsymbol{x}{k|k} xk∣k与后验协方差 P k ∣ k \boldsymbol{P}{k|k} Pk∣k，可生成 2 n + 1 2n+1 2n+1个 Sigma 点，生成公式如下。其中 n n n为状态向量 x k ∣ k \boldsymbol{x}{k|k} xk∣k的维度，例如：若 x k ∣ k = [ p x , p y ] T \boldsymbol{x}{k|k} = [p_x, p_y]^T xk∣k=[px,py]T仅包含二维位置信息，则 n = 2 n=2 n=2，对应生成 5 5 5个 Sigma 点；若 n = 5 n=5 n=5，则对应生成 11 11 11个 Sigma 点。
该生成公式中，左侧 X k ∣ k \boldsymbol{X}{k|k} Xk∣k为最终生成的 2 n + 1 2n+1 2n+1个 Sigma 点构成的矩阵；右侧 x k ∣ k \boldsymbol{x}{k|k} xk∣k为 k k k时刻的状态均值， P k ∣ k \boldsymbol{P}{k|k} Pk∣k为状态协方差。公式中 x k ∣ k + ( n + λ ) P k ∣ k \boldsymbol{x}{k|k} + \sqrt{(n+\lambda)\boldsymbol{P}{k|k}} xk∣k+(n+λ)Pk∣k 与 x k ∣ k − ( n + λ ) P k ∣ k \boldsymbol{x}{k|k} - \sqrt{(n+\lambda)\boldsymbol{P}{k|k}} xk∣k−(n+λ)Pk∣k 两组采样点关于均值点 x k ∣ k \boldsymbol{x}{k|k} xk∣k对称分布。
式中 λ \lambda λ为缩放参数，其取值决定了周边 2 n 2n 2n个 Sigma 点与中心均值点的距离，工程中常用经验取值为 λ = 3 − n \lambda = 3-n λ=3−n。
2. 非线性映射后的均值与方差求解
通过上述规则生成的 Sigma 点集，可完整表征 k k k时刻的状态概率分布。将该点集代入非线性状态转移模型 x k + 1 = f ( x k , v k ) \boldsymbol{x}_{k+1} = \boldsymbol{f}(\boldsymbol{x}_k, \boldsymbol{v}k) xk+1=f(xk,vk)（ v k \boldsymbol{v}k vk为过程噪声），完成所有 Sigma 点的一步预测，得到 x k + 1 ∣ k , i \boldsymbol{x}{k+1|k,i} xk+1∣k,i，其中 x k + 1 ∣ k , i \boldsymbol{x}{k+1|k,i} xk+1∣k,i代表预测 Sigma 点集中的第 i i i个采样点。
对所有预测后的 Sigma 点进行加权求和，即可得到 k + 1 k+1 k+1时刻的状态先验均值与协方差，其中各采样点的权值 w i w_i wi计算规则为：
w [ i ] = λ λ + n , i = 1 w [ i ] = 1 2 ( λ + n ) , i = 2 , ... , 2 n + 1 \begin{align} w^{[i]} &= \frac{\lambda}{\lambda + n}, \quad i = 1 \\ w^{[i]} &= \frac{1}{2(\lambda + n)}, \quad i = 2, \dots, 2n+1 \end{align} w[i]w[i]=λ+nλ,i=1=2(λ+n)1,i=2,...,2n+1
式中 λ = 3 − n \lambda = 3-n λ=3−n，完成上述加权运算后，即可得到预测模型的均值与协方差。后续需对量测空间完成相同的无迹变换，最终通过两个高斯分布的融合完成状态更新。
更新部分
不同类型传感器的量测模型存在差异，量测空间的维度与映射关系各不相同。本文以恒定转速与速率模型（CTRV, constant turn rate and velocity magnitude）为基础，结合激光雷达（Lidar）与毫米波雷达（Radar）的车辆跟踪场景，完成 UKF 更新环节的讲解。量测更新的逻辑与传统卡尔曼滤波一致，均为通过预测值与实测值的残差完成状态修正，差异仅体现在非线性量测的处理方式上。
参考链接
via:
- 卡尔曼滤波理论讲解与应用（matlab 和 python）_matlab+卡尔曼滤波的公式讲解和与应用-CSDN 博客
https://blog.csdn.net/O_MMMM_O/article/details/105718468 - 扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python 和 C++代码）_ekf 滤波实例-CSDN 博客
https://blog.csdn.net/O_MMMM_O/article/details/106078679 - 无迹（损）卡尔曼滤波（UKF）理论讲解与实例_无迹卡尔曼滤波估算例子-CSDN 博客
https://blog.csdn.net/O_MMMM_O/article/details/106140853