离散卡尔曼滤波总结

最新推荐文章于 2024-08-02 16:54:35 发布

原创最新推荐文章于 2024-08-02 16:54:35 发布 · 4.6k 阅读

15 ·

CC 4.0 BY-SA版权

概述

1.卡尔曼滤波目的
在干扰为高斯分布的情况下，得到测量均方误差最小，即测量扰动值最小，看起来最为平滑。
预测下一步的测量值

2.卡尔曼滤波是最优的前提

观测值和估计值均有误差，且服从高斯分布
线性微分系统

3.方法
观测值与估算值进行加权平均，两者权值和为1。
根据以往的方差来决定给予权值的大小，观测值与估计值哪个方差小，就给高权值。
利用多次观察和估计来达到准确的测量目的，算法是递归的，需要多次重复调整测量值与估计值的权重。
把测量值和估计值比较一下，如果估计值比测量值小，那就把估计值加上他们之间的偏差作为新的估计值，当然前面要加上加权系数。

$X(k∣k)=X(k∣k−1)+K(k)⋅[Z(k)−H⋅X(k∣k−1)]\rm X(k|k)=X(k|k-1)+K(k)\cdot[Z(k)-H\cdot X(k|k-1)]$
如果估计值X(k|k-1)小了，那么新的估计值会加上一个量 $K⋅(Z−X)\rm K \cdot ( Z-X)$ , 如果估计值大过Z了，那么新的估计值就会减去一个量 $K⋅(Z−X)\rm K \cdot ( Z-X)$ ，这就保证新的估计值一定比现在的准确，一次一次递归下去就会越来越准却了。当然这里面很有作用的也是这个K，也就是卡尔曼增益。
改写为：
$X(k∣k)=[I−K(k)⋅H]⋅X(k∣k−1)+K(k)⋅Z(k)\rm X(k|k)=[I-K(k)\cdot H]\cdot X(k|k-1)+K(k)\cdot Z(k)$
测量值Z(k)的权值为K(k)，估计值X(k|k-1)的权值为 $[I−K(k)⋅H]\rm [I-K(k)\cdot H]$ ，两者都取决于卡尔曼增益K(k)。究竟K(k)取多大，全看估计值的方差Q和测量值的方差R。
即：

$K(k)=P(k∣k−1)⋅H′⋅[H⋅P(k∣k−1)⋅H′+R]−1\rm K(k)=P(k|k-1)\cdot H'\cdot [H\cdot P(k|k-1)\cdot H'+R]^{-1}$
$P(k∣k−1)=A⋅P(k−1∣k−1)⋅A′+Q\rm P(k|k-1)=A\cdot P(k-1|k-1)\cdot A'+Q$

数学建模

1.测量值与系统状态的关系（矩阵相乘，限定了为线性系统）
$Z(k)=H⋅X(k)+V(k)(1)\rm Z(k)=H\cdot X(k)+V(k) \qquad (1)$

2.前一时刻状态与下一时刻状态关系（固定的转移方程，与概率无关）
$X(k∣k−1)=A⋅X(k−1∣k−1)+B⋅U(k)+W(k)(2)\rm X(k|k-1)=A\cdot X(k-1|k-1)+B\cdot U(k)+W(k) \qquad (2)$

H, A, B，这三个矩阵固定，并且是事先设定好的，全都已知
W(k)为过程噪声，服从N(0,Q)
V(k)为测量噪声，服从N(0,R)
Q为预测方差，R为测量方差

3.由测量值与估计值进行校正
先验估计X(k|k-1)与加权的测量值Z(k)和其预测 $H⋅X(k∣k−1)\rm H\cdot X(k|k-1)$ 之差的线性组合构成了后验状态估计X(k|k):
$X(k∣k)=X(k∣k−1)+K(k)⋅[Z(k)−H⋅X(k∣k−1)](3)\rm X(k|k)=X(k|k-1)+K(k)\cdot[Z(k)-H\cdot X(k|k-1)] \qquad (3)$
其中
$K(k)=P(k∣k−1)⋅H′⋅[H⋅P(k∣k−1)⋅H′+R]−1(4)\rm K(k)=P(k|k-1)\cdot H'\cdot [H\cdot P(k|k-1)\cdot H'+R]^{-1} \qquad (4)$
再其中
$P(k∣k−1)=A⋅P(k−1∣k−1)⋅A′+Q\rm P(k|k-1)=A\cdot P(k-1|k-1)\cdot A'+Q$
定义误差(真实值与估计值之间的误差)
$e(k)=X(k)−X(k∣k)\rm e(k)=X(k)-X(k|k)$
则
$P(k∣k)=cov(X(k)−X(k∣k))\rm P(k|k)=cov(X(k)-X(k|k))$
$=(I−K(k)H)⋅P(k∣k−1)\rm \qquad =(I-K(k)H)\cdot P(k|k-1)$

总结

只需给定初始估计：X(0|0)与P(0|0)，就可以进行迭代
【属于先验知识，只影响收敛速度】

一、时间更新（预测）

二、测量更新（校正）

1.计算卡尔曼增益
$K(k)=P(k∣k−1)⋅H′⋅[H⋅P(k∣k−1)⋅H′+R]−1\rm K(k)=P(k|k-1)\cdot H'\cdot [H\cdot P(k|k-1)\cdot H'+R]^{-1} \qquad$
2.结合观测值，更新估计【可认为X(k|k)为真实值】
$X(k∣k)=X(k∣k−1)+K(k)⋅[Z(k)−H⋅X(k∣k−1)]\rm X(k|k)=X(k|k-1)+K(k)\cdot[Z(k)-H\cdot X(k|k-1)] \qquad$
3.更新误差协方差
$P(k∣k)=[I−K(k)H]⋅P(k∣k−1)\rm P(k|k)=[I-K(k)H]\cdot P(k|k-1)$