1.状态转移方程和观测方程
- 状态转移方程
x k = A x k − 1 + B u k + w k x_k = A x_{k-1} + Bu_k + w_k xk=Axk−1+Buk+wk- x k x_k xk：k时刻的 状态向量，理论上的真实状态。
- x k − 1 x_{k-1} xk−1：k-1时刻的 状态向量，理论上的真实状态。
- A A A：状态转移矩阵,用来决定下一时刻 状态向量 在 状态空间 中的位置
- B , u k B, u_k B,uk：控制矩阵 和 控制向量，用来对 状态向量 的位置进行修正或控制
- w k w_k wk：过程噪声，模型本身引入的噪声，假定其符合 正态分布 ，数学期望(均值)为0，协方差矩阵为 Q Q Q，
- 观测方程
z k = H x k + v k z_k = Hx_k + v_k zk=Hxk+vk- z k z_k zk：k时刻的观测值
- H H H：观测矩阵，用于将 x k x_k xk 映射到观测空间
- x k x_k xk：与 状态转移方程 中的 x k x_k xk 意义相同
- v k v_k vk：测量噪声，测量不准确引入的噪声，假定其符合 正态分布 ，数学期望(均值)为0，协方差矩阵为 Q Q Q，
- 注意：
- 在上述两个公式中 x k x_k xk,代表 k k k 时刻的 真实值，他是一个理想值，他假定预测模型和测量是完备的，误差也是已知的，但这在现实中是不可能的，卡尔曼滤波本身也是，不断通过 预测 和 修正 两个阶段交替进行，使计算结果不断逼近真实结果 x k x_k xk 的一种方法。
- 也可以这么理解卡尔曼滤波：模型估计 和 测量 都有误差，误差的 概率分布 符合 正态分布（高斯分布），也就是 估计值 和 测量值都不准，卡尔曼滤波就是在 估计值 和 测量值 之间找到一个最优平衡点， 也就是找到 概率密度 最高的那个点，是一种最优估计的算法。
如果误差都是已知的，那就直接修正了，那就等于没有误差，也不需要滤波，一个公式，直接就得到真结果了，那显然是不可能的。
2.卡尔曼增益 K k K_k Kk
依据上述的状态方程和观测方程，可以得到下面两个公式：
x ^ k − = F x ^ k − 1 + B u k \hat{\mathbf{x}}_k^-=\mathbf{F} \hat{\mathbf{x}}_{k-1}+\mathbf{B} \mathbf{u}_k x^k−=Fx^k−1+Buk
z k = H x ^ k m e a x ^ k m e a = H − z k \mathbf{z}_k=\mathbf{H} \hat{\mathbf{x}}_{k_{mea}} \\ \hat{\mathbf{x}}_{k_{mea}} = H^-z_k zk=Hx^kmeax^kmea=H−zk- x ^ k − \hat{\mathbf{x}}_k^- x^k−：当前状态的 先验估计 ，通过模型计算出来的值。
- x ^ k m e a \hat{\mathbf{x}}_{k_{mea}} x^kmea 测量出来的值。
此时， x ^ k − \hat{\mathbf{x}}_k^- x^k− 和 x ^ k m e a \hat{\mathbf{x}}_{k_{mea}} x^kmea 都是不准的，下一步就是从这两个不准的值中找到 最优估计
假设存在一个系数 G G G，决定了我们应该相信 算出来的结果 还是 测出来的结果
x ^ k = x ^ k − + G ( H − z k − x ^ k − ) \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + G(H^-z_k - \hat{\mathbf{x}}_k^-) x^k=x^k−+G(H−zk−x^k−)- 假设 G = 0 G = 0 G=0 , x ^ k = x ^ k − \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- x^k=x^k−, 这时应该相信 算出来的结果。
- 假设 G = 1 G = 1 G=1, x ^ k = H − z k \hat{\mathbf{x}}_k = H^-z_k x^k=H−zk，这时更应该相信 测出来的结果
x ^ k \hat{\mathbf{x}}_k x^k：后验估计，也就是当前时刻的最优估计值
对这个公式进行一个变换， 是 G = k k H G = k_k H G=kkH, 带入上述公式得到
x ^ k = x ^ k − + G ( H − z k − x ^ k − ) x ^ k = x ^ k − + k k H ( H − z k − x ^ k − ) − − − − − x ^ k = x ^ k − + k k ( z k − H x ^ k − ) \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + G(H^-z_k - \hat{\mathbf{x}}_k^-) \\ \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + k_k H(H^-z_k - \hat{\mathbf{x}}_k^-) \\ -----\\ \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + k_k (z_k - H\hat{\mathbf{x}}_k^-) \\ x^k=x^k−+G(H−zk−x^k−)x^k=x^k−+kkH(H−zk−x^k−)−−−−−x^k=x^k−+kk(zk−Hx^k−)- x ^ k \hat{\mathbf{x}}_k x^k：后验估计
- 此时 K k K_k Kk 的取值范围是 0 − H − {0 - H^-} 0−H−
K k K_k Kk 就是 卡尔曼增益, 后续主要解决 k k k_k kk 的取值
3. 误差的协方差矩阵 P k P_k Pk
- 每个时刻都会计算出一个 最优的估计(后验估计) x ^ k \hat{\mathbf{x}}_k x^k， 通常情况下，后验估计 和 真实值 仍然存在误差，假设误差为 e k e_k ek :
e k = x k − x ^ k / / 真实值 − 后验估计 e_k = x_k - \hat{\mathbf{x}}_k // 真实值 - 后验估计 ek=xk−x^k//真实值−后验估计 - P k P_k Pk其实就是 e k e_k ek 的协方差矩阵
P k = E [ e k e k T ] P_k = E[e_k {e_k}^T] Pk=E[ekekT] - 我们的目标是让误差 e k e_k ek 最小，也可以理解为方差最小，也可以说是协方差矩阵 P k P_k Pk 对角线之和最小,也就是协方差矩阵的 迹 最小
- 不难理解， P k P_k Pk也是一个理想值，无法直接获得，上面描述主要是帮助理解 P k P_k Pk 是什么，卡尔曼滤波中使用下面公式估计 P k P_k Pk 的值，下面是 P k P_k Pk 的先验估计公式
P k − = F k P k − 1 F k ⊤ + Q k \mathbf{P}_k^-=\mathbf{F}_k\mathbf{P}_{k-1}\mathbf{F}_k^\top+\mathbf{Q}_k Pk−=FkPk−1Fk⊤+Qk- Q k Q_k Qk：估计误差 w k w_k wk(状态空间方程中的 w k w_k wk) 的协方差矩阵： Q k = E [ w k w k T ] Q_k = E[w_k {w_k}^T] Qk=E[wkwkT]
- 卡尔曼增益 k k k_k kk的公式
K k = P k − H T H P k − H T + R k K_k=\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}+R_k} Kk=HPk−HT+RkPk−HT- R k R_k Rk：测量误差 v k v_k vk 的协方差矩阵： R k = E [ v k v k T ] R_k = E[v_k {v_k}^T] Rk=E[vkvkT]
- 当 R k R_k Rk 比较大时，也就是分母比较大， K k K_k Kk 就会比较小，当 R k R_k Rk 足够大， K k K_k Kk 就会趋近于0。
- 当 R k R_k Rk 比较小时， K k ≈ P k − H T H P k − H T ≈ H − 1 K_k \approx {\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}}} \approx H^{-1} Kk≈HPk−HTPk−HT≈H−1
4. 卡尔曼滤波的递推流程
- 卡尔曼滤波器的递推流程主要分为 预测 和 校正 两步
- 预测部分： 包括 状态的先验估计 和 误差协方差矩阵的先验估计
x ^ k − = F x ^ k − 1 + B u k / / 状态的先验估计 \hat{\mathbf{x}}_k^-=\mathbf{F} \hat{\mathbf{x}}_{k-1}+\mathbf{B} \mathbf{u}_k //状态的先验估计 x^k−=Fx^k−1+Buk//状态的先验估计
P k − = F k P k − 1 F k ⊤ + Q k / / 误差协方差矩阵的先验估计 \mathbf{P}_k^-=\mathbf{F}_k\mathbf{P}_{k-1}\mathbf{F}_k^\top+\mathbf{Q}_k // 误差协方差矩阵的先验估计 Pk−=FkPk−1Fk⊤+Qk//误差协方差矩阵的先验估计 - 矫正部分包括：卡尔曼增益的计算， 后验估计(当前时刻的最优估计)， 更新误差的协方差矩阵
K k = P k − H T H P k − H T + R k / / 卡尔曼增益 K_k=\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}+R_k} //卡尔曼增益 Kk=HPk−HT+RkPk−HT//卡尔曼增益
x ^ k = x ^ k − + k k ( z k − H x ^ k − ) / / 后验估计 ( 当前时刻的最优估计 ) \hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + k_k (z_k - H\hat{\mathbf{x}}_k^-) //后验估计(当前时刻的最优估计) x^k=x^k−+kk(zk−Hx^k−)//后验估计(当前时刻的最优估计)
P k = ( I − K k H ) P k − / / 更新误差的协方差矩阵 ; I : 单位矩阵 P_{k}=(I-K_{k}H)P_{k}^{-} // 更新误差的协方差矩阵 ; I:单位矩阵 Pk=(I−KkH)Pk−//更新误差的协方差矩阵;I:单位矩阵