离散卡尔曼滤波总结

概述

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(kk)=X(kk1)+K(k)[Z(k)HX(kk1)]
如果估计值X(k|k-1)小了,那么新的估计值会加上一个量 K⋅(Z−X)\rm K \cdot ( Z-X)K(ZX), 如果估计值大过Z了,那么新的估计值就会减去一个量K⋅(Z−X)\rm K \cdot ( Z-X)K(ZX),这就保证新的估计值一定比现在的准确,一次一次递归下去就会越来越准却了。当然这里面很有作用的也是这个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(kk)=[IK(k)H]X(kk1)+K(k)Z(k)
测量值Z(k)的权值为K(k),估计值X(k|k-1)的权值为[I−K(k)⋅H]\rm [I-K(k)\cdot H][IK(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(kk1)H[HP(kk1)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(kk1)=AP(k1k1)A+Q

数学建模

1.测量值与系统状态的关系(矩阵相乘,限定了为线性系统
Z(k)=H⋅X(k)+V(k)(1)\rm Z(k)=H\cdot X(k)+V(k) \qquad (1)Z(k)=HX(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(kk1)=AX(k1k1)+BU(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)HX(kk1)之差的线性组合构成了后验状态估计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(kk)=X(kk1)+K(k)[Z(k)HX(kk1)](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(kk1)H[HP(kk1)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(kk1)=AP(k1k1)A+Q
定义误差(真实值与估计值之间的误差)
e(k)=X(k)−X(k∣k)\rm e(k)=X(k)-X(k|k) e(k)=X(k)X(kk)

P(k∣k)=cov(X(k)−X(k∣k))\rm P(k|k)=cov(X(k)-X(k|k))P(kk)=cov(X(k)X(kk))
=(I−K(k)H)⋅P(k∣k−1)\rm \qquad =(I-K(k)H)\cdot P(k|k-1)=(IK(k)H)P(kk1)

总结

只需给定初始估计: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(kk1)=AX(k1k1)+BU(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(kk1)=AP(k1k1)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(kk1)H[HP(kk1)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(kk)=X(kk1)+K(k)[Z(k)HX(kk1)]
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(kk)=[IK(k)H]P(kk1)

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值