概述
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)=X(k∣k−1)+K(k)⋅[Z(k)−H⋅X(k∣k−1)]
如果估计值X(k|k-1)小了,那么新的估计值会加上一个量 K⋅(Z−X)\rm K \cdot ( Z-X)K⋅(Z−X), 如果估计值大过Z了,那么新的估计值就会减去一个量K⋅(Z−X)\rm K \cdot ( Z-X)K⋅(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)X(k∣k)=[I−K(k)⋅H]⋅X(k∣k−1)+K(k)⋅Z(k)
测量值Z(k)的权值为K(k),估计值X(k|k-1)的权值为[I−K(k)⋅H]\rm [I-K(k)\cdot H][I−K(k)⋅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} K(k)=P(k∣k−1)⋅H′⋅[H⋅P(k∣k−1)⋅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'+QP(k∣k−1)=A⋅P(k−1∣k−1)⋅A′+Q
数学建模
1.测量值与系统状态的关系(矩阵相乘,限定了为线性系统)
Z(k)=H⋅X(k)+V(k)(1)\rm Z(k)=H\cdot X(k)+V(k) \qquad (1)Z(k)=H⋅X(k)+V(k)(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)X(k∣k−1)=A⋅X(k−1∣k−1)+B⋅U(k)+W(k)(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)H⋅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)X(k∣k)=X(k∣k−1)+K(k)⋅[Z(k)−H⋅X(k∣k−1)](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)K(k)=P(k∣k−1)⋅H′⋅[H⋅P(k∣k−1)⋅H′+R]−1(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'+QP(k∣k−1)=A⋅P(k−1∣k−1)⋅A′+Q
定义误差(真实值与估计值之间的误差)
e(k)=X(k)−X(k∣k)\rm e(k)=X(k)-X(k|k) 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))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)=(I−K(k)H)⋅P(k∣k−1)
总结
只需给定初始估计:X(0|0)与P(0|0),就可以进行迭代
【属于先验知识,只影响收敛速度】
一、时间更新(预测)
【整个过程只需迭代计算X(k|k-1)与P(k|k-1)两个值】
1.向前推算状态变量
X(k∣k−1)=A⋅X(k−1∣k−1)+B⋅U(k)+W(k)\rm X(k|k-1)=A\cdot X(k-1|k-1)+B\cdot U(k)+W(k) \qquad X(k∣k−1)=A⋅X(k−1∣k−1)+B⋅U(k)+W(k)
2.向前推算误差协方差
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'+QP(k∣k−1)=A⋅P(k−1∣k−1)⋅A′+Q
二、测量更新(校正)
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 K(k)=P(k∣k−1)⋅H′⋅[H⋅P(k∣k−1)⋅H′+R]−1
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 X(k∣k)=X(k∣k−1)+K(k)⋅[Z(k)−H⋅X(k∣k−1)]
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)P(k∣k)=[I−K(k)H]⋅P(k∣k−1)