拓展卡尔曼滤波器(EKF)的数学推导

基本原理

1.局部线性化过程
2.线性kalman滤波

同一般的卡尔曼滤波器(KF)不同的是,扩展卡尔曼滤波器(EKF)是为了解决 在非线性系统中,状态噪声和测量噪声均为高斯分布时,进行最优的状态估计的问题。

基本思想:将非线性系统滤波问题近似化为线性系统的滤波问题。具体做法是将非线性的状态方程f(∗)f(*)f(∗)和测量方程h(∗)h(*)h(∗)在状态值Xk^\hat{X_k}Xk^处进行泰勒展开,并且忽略掉二阶及以上项。转化为线性系统滤波,最后用Kalman滤波进行状态估计。
用途:导弹，卫星，飞机等系统的状态估计问题。

1.局部线性化过程

一般的离散非线性系统大概可以表示成下面的式子:
X(k+1)=f[X(k),k]+G(k)W(k)(1)Z(k)=h[X(k),k]+V(k)(2)X(k+1) = f[X(k),k]+G(k)W(k) \ \ (1)\\Z(k)=h[X(k),k]+V(k) \ \ (2)X(k+1)=f[X(k),k]+G(k)W(k) (1)Z(k)=h[X(k),k]+V(k) (2)
其中W(k)W(k)W(k)为状态噪声,V(k)V(k)V(k)为观测噪声,其状态噪声的协方差矩阵为QQQ,观测噪声的协方差矩阵为RRR。
对于(1)在先验估计Xk^\hat{X_k}Xk^处进行一阶泰勒展开:
X(k+1)=f(k,Xk^)+∂f∂Xk^(Xk−Xk^)+G(k)W(k)X(k+1)=f(k,\hat{X_k})+\frac{\partial{f}}{\partial{\hat{X_k}}}(X_k-\hat{X_k})+G(k)W(k) X(k+1)=f(k,Xk^)+∂Xk^∂f(Xk−Xk^)+G(k)W(k)在这里我们设:
Φ(k+1∣k)=∂f∂Xk^=∂f∂X∣X=Xk^ϕ(k)=f(k,Xk^)−∂f∂XkXk^\Phi(k+1|k)=\frac{\partial{f}}{\partial{\hat{X_k}}}=\frac{\partial{f}}{\partial{X}}|_{X=\hat{X_k}}\\ \phi(k)=f(k,\hat{X_k})-\frac{\partial{f}}{\partial{X_k}}\hat{X_k}Φ(k+1∣k)=∂Xk^∂f=∂X∂f∣X=Xk^ϕ(k)=f(k,Xk^)−∂Xk∂fXk^因此可以将状态方程化为:
X(k+1)=Φ(k+1∣k)X(k)+ϕ(k)+G(k)W(k)X(k+1)=\Phi(k+1|k)X(k)+\phi(k)+G(k)W(k)X(k+1)=Φ(k+1∣k)X(k)+ϕ(k)+G(k)W(k)其中Φ(k+1∣k)\Phi(k+1|k)Φ(k+1∣k)被称为状态转移矩阵:Φ(k+1∣k)=∂f∂Xk^=(∂f1∂x1∂f1∂x2∂f1∂x3...∂f1∂xn∂f2∂x1∂f2∂x2∂f2∂x3...∂f2∂xn...∂fn∂x1∂fn∂x2∂fn∂x3...∂fn∂xn)∣X=Xk^\Phi(k+1|k)=\frac{\partial{f}}{\partial{\hat{X_k}}}=\begin{pmatrix} \frac{\partial{f_1}}{\partial{x_1}} & \frac{\partial{f_1}}{\partial{x_2}} & \frac{\partial{f_1}}{\partial{x_3}}...& \frac{\partial{f_1}}{\partial{x_n}}\\ \frac{\partial{f_2}}{\partial{x_1}} & \frac{\partial{f_2}}{\partial{x_2}} & \frac{\partial{f_2}}{\partial{x_3}}...& \frac{\partial{f_2}}{\partial{x_n}} \\...\\ \frac{\partial{f_n}}{\partial{x_1}} & \frac{\partial{f_n}}{\partial{x_2}} & \frac{\partial{f_n}}{\partial{x_3}}...& \frac{\partial{f_n}}{\partial{x_n}} \end{pmatrix}|_{X=\hat{X_k}} Φ(k+1∣k)=∂Xk^∂f=⎝⎜⎜⎜⎛∂x1∂f1∂x1∂f2...∂x1∂fn∂x2∂f1∂x2∂f2∂x2∂fn∂x3∂f1...∂x3∂f2...∂x3∂fn...∂xn∂f1∂xn∂f2∂xn∂fn⎠⎟⎟⎟⎞∣X=Xk^同样的,将(2)在Xk^\hat{X_k}Xk^处同样可以进行一阶泰勒展开,得到:Z(k)=H(k)X(k)+y(k)+V(k)Z(k)=H(k)X(k)+y(k)+V(k)Z(k)=H(k)X(k)+y(k)+V(k)其中:H(k)=∂h∂Xk^=(∂h1∂x1∂h1∂x2∂h1∂x3...∂h1∂xn∂h2∂x1∂h2∂x2∂h2∂x3...∂h2∂xn...∂hn∂x1∂hn∂x2∂hn∂x3...∂hn∂xn)∣X=Xk^H(k)=\frac{\partial{h}}{\partial{\hat{X_k}}}=\begin{pmatrix} \frac{\partial{h_1}}{\partial{x_1}} & \frac{\partial{h_1}}{\partial{x_2}} & \frac{\partial{h_1}}{\partial{x_3}}...& \frac{\partial{h_1}}{\partial{x_n}}\\ \frac{\partial{h_2}}{\partial{x_1}} & \frac{\partial{h_2}}{\partial{x_2}} & \frac{\partial{h_2}}{\partial{x_3}}...& \frac{\partial{h_2}}{\partial{x_n}} \\...\\ \frac{\partial{h_n}}{\partial{x_1}} & \frac{\partial{h_n}}{\partial{x_2}} & \frac{\partial{h_n}}{\partial{x_3}}...& \frac{\partial{h_n}}{\partial{x_n}} \end{pmatrix}|_{X=\hat{X_k}}H(k)=∂Xk^∂h=⎝⎜⎜⎛∂x1∂h1∂x1∂h2...∂x1∂hn∂x2∂h1∂x2∂h2∂x2∂hn∂x3∂h1...∂x3∂h2...∂x3∂hn...∂xn∂h1∂xn∂h2∂xn∂hn⎠⎟⎟⎞∣X=Xk^

2.线性kalman滤波

将非线性的方程转化成线性之后就可以进行kalman滤波了:
X^k−=f(X^k−1)P^k−=Φ(k∣k−1)P^kΦT(k∣k−1)+Gk−QkGk−TKk=P^k−HkT(HkP^k−HkT+Rk)−1X^k=X^k−+Kk(zk−h(X^k−))Pk^=(I−KkHk)P^k−\hat{X}_{k}^-=f(\hat{X}_{k-1})\\ \hat{P}_k^-=\Phi(k|k-1)\hat{P}_k\Phi^T(k|k-1)+G_{k}^-Q_kG_{k}^{-T}\\ K_k=\hat{P}_k^-H_k^T(H_k\hat{P}_k^-H_k^T+R_k)^{-1} \\ \hat{X}_k=\hat{X}_{k}^-+K_k(z_k-h(\hat{X}_k^-)) \\ \hat{P_k}=(I-K_kH_k)\hat{P}_k^-X^k−=f(X^k−1)P^k−=Φ(k∣k−1)P^kΦT(k∣k−1)+Gk−QkGk−TKk=P^k−HkT(HkP^k−HkT+Rk)−1X^k=X^k−+Kk(zk−h(X^k−))Pk^=(I−KkHk)P^k−以上就是其推导的总过程。