卡尔曼滤波与扩展卡尔曼滤波(EKF)
本文仅用于个人学习的总结。
0. 参考资料
图说卡尔曼滤波，一份通俗易懂的教程：https://zhuanlan.zhihu.com/p/39912633
在此之上的一个解释：https://zhuanlan.zhihu.com/p/81404580
1. 基础概念
当物体在运动时，根据当前时刻的位置，和运动方程，可以推测出下一个时刻的位置。当到达下一个位置时，能够通过传感器观测到一个位置，两个位置进行融合，得到最终的位置。
2. 数学
2.1 符号定义：
x ^ k \hat{x}_k x^k：k时刻的待估计状态
F k F_k Fk：k时刻的状态转移矩阵
P k P_k Pk：k时刻状态的协方差矩阵
B k B_k Bk：k时刻控制矩阵
u k u_k uk：k时刻控制向量
Q k Q_k Qk：k时刻环境的不确定度协方差矩阵
z k z_k zk：观测数据
R k R_k Rk：观测数据的协方差矩阵
H k H_k Hk：观测矩阵
K K K：卡尔曼增益
2.2 预测
所谓预测，是指在根据前一个状态预测下一个状态，所需要预测的内容包括：位置与相应的协方差矩阵。我们认为状态量是一个高斯分布。即： ( μ , Σ ) ∼ N ( x ^ , P ) (\mu, \Sigma) \sim \mathcal N(\hat{x}, P) (μ,Σ)∼N(x^,P)
最理想情况
状态转移： x ^ k = F k x ^ k − 1 \hat{x}_k=F_k \hat{x}_{k-1} x^k=Fkx^k−1
协方差矩阵递推： P k = F k P k − 1 F k T P_k=F_k P_{k-1} F_k^T Pk=FkPk−1FkT
（注意：若 C o v ( x ) = Σ , 则 C o v ( A x ) = A Σ A T Cov(x)=\Sigma, 则Cov(Ax)=A \Sigma A^T Cov(x)=Σ,则Cov(Ax)=AΣAT）
当有外部输入时，比如有外力作用，状态不仅仅是一个简单的转移，需要加上运动量 B k u k B_k u_k Bkuk，此时更新为：
状态转移： x ^ k = F k x ^ k − 1 + B k u k \hat{x}_k=F_k \hat{x}_{k-1}+B_k u_k x^k=Fkx^k−1+Bkuk
当环境存在不确定性时
即虽然可能运动方程没有错误，但受到环境扰动，最终到达位置是不确定的。这个不确定和之前的不确定并不一样，之前的协方差矩阵表示当前位置的不确定，而这里的环境不确定性则是假设之前位置确定后重新推算出的位置不确定，所以可以理解成在原来“不确定的分布区间”中的每一个点在映射过程中仍有了新的不确定性，“不确定的分布区间”从而变大。
此时协方差递推为： P k = F k P k − 1 F k T + Q k P_k=F_k P_{k-1} F_k^T+Q_k Pk=FkPk−1FkT+Qk
一般来说， Q k Q_k Qk 是非负定的，即不肯能由于噪声而使不确定度更小。
2.3 修正（更新）
所谓更新，或修正，是指在原有推测的状态基础上，根据传感器的观测数据，得到新的状态和协方差的递推。我们依旧认为观测数据是一个正态分布，满足 ( μ , Σ ) ∼ N ( z ^ , R ) (\mu, \Sigma) \sim \mathcal N(\hat{z}, R) (μ,Σ)∼N(z^,R)。
这里的观测数据是由当前状态 x ^ \hat{x} x^ 测得的，通过观测矩阵 H H H，获得实际的观测到的 z z z，从而以上分布可以写成：
( μ , Σ ) ∼ N ( H x ^ , R ) (\mu, \Sigma) \sim \mathcal N(H\hat{x}, R) (μ,Σ)∼N(Hx^,R) 那么预测结果如何和观测结果合并呢？
采用相乘的方式，得到新的正态分布： N ( x , μ 0 , Σ 0 ) ∗ N ( x , μ 1 , Σ 1 ) = N ( x , μ ′ , Σ ′ ) \mathcal N(x, \mu_0, \Sigma_0)*\mathcal N(x, \mu_1,\Sigma_1) = \mathcal N(x, \mu', \Sigma') N(x,μ0,Σ0)∗N(x,μ1,Σ1)=N(x,μ′,Σ′) 其中 K = Σ 0 ( Σ 0 + Σ 1 ) − 1 K=\Sigma_0(\Sigma_0+\Sigma_1)^{-1} K=Σ0(Σ0+Σ1)−1 μ ′ = μ 0 + K ( μ 1 − μ 0 ) \mu'=\mu_0+K(\mu_1-\mu_0) μ′=μ0+K(μ1−μ0) Σ ′ = Σ 0 − K Σ 0 \Sigma'=\Sigma_0-K\Sigma_0 Σ′=Σ0−KΣ0 带入实际的分布，可以得到： H k x ^ k ′ = H k x ^ k + K ( z k − H k x ^ k ) H_k \hat{x}_k'=H_k\hat{x}_k+K(z_k -H_k\hat{x}_k) Hkx^k′=Hkx^k+K(zk−Hkx^k) H k P k ′ H k T = H k P k H k T − K H k P k H k T H_kP_k'H_k^T=H_kP_kH_k^T-KH_kP_kH_k^T HkPk′HkT=HkPkHkT−KHkPkHkT 其中卡尔曼增益为： K = H k P k H k T ( H k P k H k T + R k ) − 1 K=H_kP_kH_k^T(H_kP_kH_k^T+R_k)^{-1} K=HkPkHkT(HkPkHkT+Rk)−1 整理得到： x ^ k ′ = x ^ k + K ( z k − H k x ^ k ) \hat{x}_k'=\hat{x}_k+K(z_k -H_k\hat{x}_k) x^k′=x^k+K(zk−Hkx^k) P k ′ = P k − K H k P k P_k'=P_k-KH_kP_k Pk′=Pk−KHkPk K = P k H k T ( H k P k H k T + R k ) − 1 K=P_kH_k^T(H_kP_kH_k^T+R_k)^{-1} K=PkHkT(HkPkHkT+Rk)−1 即完成了更新部分
2.4 总结
由于假设了预测是正态分布，更新时的观测也是正态分布，可以直接将两个正态分布合并成一个新的正态分布，作为下一次预测的前一个时刻，从而不断进行。
正是由于假设正态分布，才能够不断递推进行。其中从协方差外加状态转移矩阵时可以看出，矩阵作为线性变换是不影正态性的。
总结卡尔曼滤波五个等式
预测： x ^ k = F k x ^ k − 1 + B k u k \hat{x}_k=F_k \hat{x}_{k-1}+B_k u_k x^k=Fkx^k−1+Bkuk P k = F k P k − 1 F k T + Q k P_k=F_k P_{k-1} F_k^T+Q_k Pk=FkPk−1FkT+Qk 修正： x ^ k ′ = x ^ k + K ( z k − H k x ^ k ) \hat{x}_k'=\hat{x}_k+K(z_k -H_k\hat{x}_k) x^k′=x^k+K(zk−Hkx^k) P k ′ = P k − K H k P k P_k'=P_k-KH_kP_k Pk′=Pk−KHkPk K = P k H k T ( H k P k H k T + R k ) − 1 K=P_kH_k^T(H_kP_kH_k^T+R_k)^{-1} K=PkHkT(HkPkHkT+Rk)−1
当状态转移、观测等不是线性时，就无法保持正态性，所以需要采用扩展卡尔曼滤波(EKF)。
3. 扩展卡尔曼滤波
当状态转移和控制，不是简单的矩阵，而是非线性的函数关系时，例如： x k = f ( x k − 1 , u k − 1 ) + w k x_k=f(x_{k-1}, u_{k-1})+w_k xk=f(xk−1,uk−1)+wk，此时不能够继续用正态分布叠加。
此时将f函数进行taylor展开，采用一阶近似:
对于向量，一阶导即雅克比矩阵。我们使用雅克比矩阵代替卡尔曼滤波中的线性矩阵 F F F等即可。从而得到扩展卡尔曼滤波。