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