卡尔曼滤波及非线性系统估计兼谈KF、EKF、UKF

预备知识

1,n维状态xix_i有Gauss分布协方差PiP_i,有如下线性变换
xk=Axk1 x_k=Ax_{k-1}
则,xkx_k亦有Gauss分布,且协方差为APk1ATAP_{k-1}A^T
A:(xk1,Pk1)(Axk1,APk1AT)A:(x_{k-1},P_{k-1}) \to (Ax_{k-1},AP_{k-1}A^T)
2,两个Gauss分布的融合:
(u2,σ2)={(u0,σ0)(u1,σ1)K=σ0(σ0+σ1)1u2=u0+K(u1u0)σ2=σ0σ1(σ0+σ1)1=(1k)σ0 (u_2,\sigma_2)= \begin{cases} (u_0,\sigma_0) \\ (u_1,\sigma_1) \end{cases} \\ K=\sigma_0(\sigma_0+\sigma_1)^{-1} \\ u_2=u_0+K(u_1-u_0) \\ \sigma_2=\sigma_0\sigma_1(\sigma_0+\sigma_1)^{-1}=(1-k)\sigma_0
3,构造sigma点
采用对称采样策略,对分布为(xˉ,Px)(\bar{x},P_x)nn维随机变量xx产生2n+12n+1个Gauss分布列向量,如下:
xi={xˉ,i=0xˉ+[(n+κ)Px]i,i=1,2, ,nxˉ[(n+κ)Px]in,i=n+1,n+2, ,2n x^i= \begin{cases} \bar{x},i=0 \\ \bar{x}+[\sqrt{(n+\kappa)P_x}]_i,i=1,2,\cdots,n \\ \bar{x}-[\sqrt{(n+\kappa)P_x}]_{i-n},i=n+1,n+2,\cdots,2n \end{cases}
κ\kappa是尺度参数,调节其可以逼近精度。
均值权重WimW_i^m与方差权重WicW_i^c如下求取:
{W0m=κ/(n+κ)W0c=κ/(n+κ)+(1α2+β)Wim=Wic=κ/[2(n+κ)],i=1,2, ,2n \begin{cases} W_0^m=\kappa / (n+\kappa) \\ W_0^c=\kappa / (n+\kappa)+(1-\alpha^2+\beta) \\ W_i^m=W_i^c=\kappa / [2(n+\kappa)],i=1,2,\cdots,2n \end{cases}
其中,κ=α2(n+λ)n\kappa=\alpha^2(n+\lambda)-n

  • α\alpha确定xˉ\bar{x}周围sigma点的分布,通常设为较小的正数(1>α1e4)(1\gt \alpha \ge 1e^{-4} )
  • λ\lambda是第二个尺度参数,通常设置为003n3-n
  • β\beta是状态分布参数,对于Gauss分布β=2\beta=2是最优的,如果状态是单变量,则β=0\beta=0最佳
  • 对于Gauss分布,考虑到4阶距的统计量,通常取值n+κ=3n+\kappa=3

卡尔曼滤波KF

状态转移方程:
xk=Fkxk1+Bkuk,QkGaussNoise x_k=F_kx_{k-1}+B_ku_k,Q_k \in GaussNoise
Pk=FkPk1FkT+QkP_k=F_kP_{k-1}F_k^T+Q_k
观测方程:
zk=Hkxk,RkGaussNoise z_k=H_kx_k,R_k \in GaussNoise
经过状态转移与观测方程,得到Gauss分布:
(Hkx^k,HkP^kHkT)(H_k\hat{x}_k, H_k\hat{P}_kH_k^T)
由传感器观测,得到Gauss分布:
(zk,Rk)(z_k, R_k)
融合这两个分布:
Kk=HkP^kHkT(HkP^kHkT+Rk)1K_k= H_k\hat{P}_kH_k^T( H_k\hat{P}_kH_k^T+R_k)^{-1}
Hkxk=Hkx^k+Kk(zkHkx^k)H_kx_k=H_k\hat{x}_k+K_k(z_k-H_k\hat{x}_k)
HkPkHkT=HkP^kHkTKkHkP^kHkTH_kP_kH_k^T=H_k\hat{P}_kH_k^T-K_kH_k\hat{P}_kH_k^T
Kk=P^kHkT(HkP^kHkT+Rk)1K_k^\prime= \hat{P}_kH_k^T( H_k\hat{P}_kH_k^T+R_k)^{-1}
得到:
xk=x^k+Kk(zkHkx^k)x_k=\hat{x}_k+K_k^\prime(z_k-H_k\hat{x}_k)
Pk=P^kKkHkP^kP_k=\hat{P}_k-K_k^\prime H_k\hat{P}_k

扩展卡尔曼滤波EKF

对于非线性系统,不能用KF,很自然的可以利用Taylor展开,转为近似线性系统,套用KF,FkF_kHkH_k需要用一阶Taylor展开的Jacobi代替。

非线性系统滤波原理

系统方程如下:
{xk=f(xk1,uk,vk)zk=g(xk,wk) \begin{cases} x_k=f(x_{k-1},u_k, v_k) \\ z_k=g(x_k,w_k) \end{cases}
系统状态转移后的统计量:
{xˉk=E{xkYk1}Pˉk=E{(xkxˉk)(xkxˉk)TYk1} \begin{cases} \bar{x}_k=E\{x_k|Y^{k-1}\} \\ \bar{P}_k=E\{(x_k- \bar{x}_k)(x_k- \bar{x}_k)^T|Y^{k-1}\} \end{cases}
系统观测的统计量:
{yˉk=E{ykYk1}Py,k=E{(ykg(xˉk))(ykg(xˉk))TYk1}Pxy,k=E{(xkxˉk)(ykg(xˉk))TYk1}Kk=Pxy,kPy,k1 \begin{cases} \bar{y}_k=E\{y_k|Y^{k-1}\} \\ P_{y,k}=E\{(y_k-g( \bar{x}_k))(y_k- g( \bar{x}_k))^T|Y^{k-1}\} \\ P_{xy,k}=E\{(x_k- \bar{x}_k)(y_k-g( \bar{x}_k))^T|Y^{k-1}\} \\ K_k=P_{xy,k}P_{y,k}^{-1} \end{cases}
系统状态更新:
{x^k=xˉk+Kk(ykyˉk)P^k=PˉkKkPy,kKkT \begin{cases} \hat{x}_k=\bar{x}_k+K_k(y_k-\bar{y}_k) \\ \hat{P}_k= \bar{P}_k-K_kP_{y,k}K_k^T \end{cases}

无迹卡尔曼滤波UKF

已知分布(x^k1,P^k1)(\hat{x}_{k-1},\hat{P}_{k-1})
取点并状态转移:
{sigma:xk1i},i0,1,2, ,2n1,2nf:xk1ixkk1i \{sigma:x_{k-1}^i\},i\in0,1,2,\cdots,2n-1,2n \\ f:x_{k-1}^i\mapsto x_{k|k-1}^i
xkk1i    {Wmx^kk1=i=02nWimxkk1iWcP^kk1=i=02nWic(xkk1ix^kk1)(xkk1ix^kk1)T+Qk x_{k|k-1}^i\implies \begin{cases} W^m\mapsto \hat{x}_{k|k-1}=\sum_{i=0}^{2n}W_i^m x_{k|k-1}^i \\ W^c\mapsto \hat{P}_{k|k-1}= \sum_{i=0}^{2n}W_i^c (x_{k|k-1}^i-\hat{x}_{k|k-1})(x_{k|k-1}^i-\hat{x}_{k|k-1})^T+Q_k \end{cases}
取点并观测转移:
{sigma:x^kk1i},i0,1,2, ,2n1,2ng:x^kk1izki \{sigma:\hat{x}_{k|k-1}^i\},i\in0,1,2,\cdots,2n-1,2n \\ g:\hat{x}_{k|k-1}^i\mapsto z_k^i
zki    {Wmz^k=i=02nWimzkiWcP^zk=i=02nWic(zkiz^k)(zkiz^k)T+RkWcP^xkzk=i=02nWic(x^kk1ix^kk1)(zkiz^k)TKk=P^xkzkP^zk1 z_k^i\implies \begin{cases} W^m\mapsto \hat{z}_k=\sum_{i=0}^{2n}W_i^m z_k^i \\ W^c\mapsto \hat{P}_{z_k}= \sum_{i=0}^{2n}W_i^c (z_k^i-\hat{z}_k)(z_k^i-\hat{z}_k)^T+R_k \\ W^c\mapsto \hat{P}_{x_kz_k}=\sum_{i=0}^{2n}W_i^c (\hat{x}_{k|k-1}^i-\hat{x}_{k|k-1})(z_k^i-\hat{z}_k)^T \end{cases} \\ K_k=\hat{P}_{x_kz_k}\hat{P}_{z_k}^{-1}
状态更新:
x^k=x^kk1+Kk(zkz^k)P^k=P^kk1KkP^zkKkT \hat{x}_k=\hat{x}_{k|k-1}+K_k(z_k-\hat{z}_k) \\ \hat{P}_k=\hat{P}_{k|k-1}-K_k\hat{P}_{z_k}K_k^T

发布了103 篇原创文章 · 获赞 66 · 访问量 89万+
展开阅读全文

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: 编程工作室 设计师: CSDN官方博客

分享到微信朋友圈

×

扫一扫,手机浏览