- 前言
- 非线性系统
- 状态/观测方程线性化
- 扩展卡尔曼滤波EKF
- 后记
经典卡尔曼滤波的使用场景是线性系统，但现实应用时，大多数系统都是非线性的。扩展卡尔曼滤波（Extended Kalman Filter）是针对非线性系统的卡尔曼滤波方法。
非线性系统经典卡尔曼滤波的状态和观测方程都是线性方程： x n = A x n − 1 + B u n + w n z n = H x n + v n x_n=Ax_{n-1}+Bu_n+w_n \\ z_n=Hx_n + v_n xn=Axn−1+Bun+wnzn=Hxn+vn
如果状态方程或者观测方程是非线性的，则有： x n = f ( x n − 1 , u n ) + w n z n = g ( x n ) + v n x_n=f(x_{n-1},u_n)+w_n \\ z_n=g(x_n)+v_n xn=f(xn−1,un)+wnzn=g(xn)+vn 由于控制输入并不本质，后续讨论不会涉及 u n u_n un。
状态/观测方程线性化可以根据泰勒公式，将函数展开为n阶多项式拟合： f ( x ) = f ( x 0 ) + f ′ ( x 0 ) ( x − x 0 ) + 1 2 f ′ ′ ( x 0 ) ( x − x 0 ) 2 + o ( ( x − x 0 ) 2 ) f(x)=f(x_0)+f'(x_0)(x-x_0)+ \frac {1}{2}f''(x_0)(x-x_0)^2+o((x-x_0)^2) f(x)=f(x0)+f′(x0)(x−x0)+21f′′(x0)(x−x0)2+o((x−x0)2)
因此，可以通过泰勒公式将非线性的状态方程和观测方程进行线性化： x n = f ( x ^ n − 1 ) + f ′ ( x ^ n − 1 ) ( x n − 1 − x ^ n − 1 ) + w n z n = g ( x ^ n ∣ n − 1 ) + g ′ ( x ^ n ∣ n − 1 ) ( x n − x ^ n ∣ n − 1 ) + v n x_n=f(\hat x_{n-1})+f'(\hat x_{n-1})(x_{n-1}- \hat x_{n-1})+w_n \\ z_n = g(\hat x_{n|n-1})+g'(\hat x_{n|n-1})(x_n - \hat x_{n|n-1}) +v_n xn=f(x^n−1)+f′(x^n−1)(xn−1−x^n−1)+wnzn=g(x^n∣n−1)+g′(x^n∣n−1)(xn−x^n∣n−1)+vn 以上线性化有几点值得注意：
- EKF线性化使用了一阶近似，算是相对简单；也可以使用二阶或者更高阶的近似。
- 由于卡尔曼增益需要计算测量残差，因此观测方程是由状态更新点 x ^ n ∣ n − 1 \hat x_{n|n-1} x^n∣n−1展开的。
将系统的状态方程和观测方程线性化后，就可以直接通过线性卡尔曼滤波的方法开始推导了，过程是完全一样的。
扩展卡尔曼滤波EKF首先，把线性卡尔曼滤波的五个公式列出来： K F : p r e d i c t i o n : x ^ n ∣ n − 1 = A x ^ n − 1 P ^ n ∣ n − 1 = A P ^ n − 1 A T + Q u p d a t e : K n = P ^ n ∣ n − 1 H T ( H P n H T + R ) − 1 x ^ n = x ^ n ∣ n − 1 + K k ( z ^ n − H x ^ n ∣ n − 1 ) P n = ( E − K k H ) P ^ n ∣ n − 1 \begin{aligned} KF: \\ prediction : & \\ & \hat x_{n|n-1} = A\hat x_{n-1} &\quad \\ & \hat P_{n|n-1}=A\hat P_{n-1}A^T+Q &\quad \\ update: & \\ & K_n = \hat P_{n|n-1}H^T(HP_nH^T+R)^{-1} \\ & \hat x_n = \hat x_{n|n-1} + K_k(\hat z_n - H \hat x_{n|n-1}) \\ & P_n = (E-K_kH)\hat P_{n|n-1} \end{aligned} KF:prediction:update:x^n∣n−1=Ax^n−1P^n∣n−1=AP^n−1AT+QKn=P^n∣n−1HT(HPnHT+R)−1x^n=x^n∣n−1+Kk(z^n−Hx^n∣n−1)Pn=(E−KkH)P^n∣n−1
扩展卡尔曼滤波，可以直接通过非线性化，将 f ( x ) , g ( x ) f(x),g(x) f(x),g(x)这样的非线性函数，近似为局部区域内的线性函数： f ′ ( x ^ n − 1 ) = A x ∼ A g ′ ( x ^ n − 1 ) = H x ∼ H f'(\hat x_{n-1})=A_x \sim A \\ g'(\hat x_{n-1})=H_x \sim H \\ f′(x^n−1)=Ax∼Ag′(x^n−1)=Hx∼H
注意，测量误差可以直接使用非线性观测方程获得： z ^ n ∣ n − 1 = g ( x ^ n ∣ n − 1 ) z ^ n − z ^ n ∣ n − 1 = z ^ n − g ( x ^ n ∣ n − 1 ) \hat z_{n|n-1}=g(\hat x_{n|n-1})\\ \hat z_n - \hat z_{n|n-1}=\hat z_n-g(\hat x_{n|n-1}) z^n∣n−1=g(x^n∣n−1)z^n−z^n∣n−1=z^n−g(x^n∣n−1)
然后，将线性卡尔曼滤波五个公式的参数进行替换，就获得了扩展卡尔曼滤波： E K F : p r e d i c t i o n : x ^ n ∣ n − 1 = f ( x ^ n − 1 ) P ^ n ∣ n − 1 = A x P ^ n − 1 A x T + Q u p d a t e : K n = P ^ n ∣ n − 1 H x T ( H x P n H x T + R ) − 1 x ^ n = x ^ n ∣ n − 1 + K k ( z ^ n − g ( x ^ n ∣ n − 1 ) ) P n = ( E − K k H x ) P ^ n ∣ n − 1 w h e r e : f ′ ( x ^ n − 1 ) = A x g ′ ( x ^ n − 1 ) = H x \begin{aligned} EKF: \\ prediction : & \\ & \hat x_{n|n-1} = f(\hat x_{n-1}) &\quad \\ & \hat P_{n|n-1}=A_x\hat P_{n-1}A_x^T+Q &\quad \\ update: & \\ & K_n = \hat P_{n|n-1}H_x^T(H_xP_nH_x^T+R)^{-1} \\ & \hat x_n = \hat x_{n|n-1} + K_k(\hat z_n - g(\hat x_{n|n-1})) \\ & P_n = (E-K_kH_x)\hat P_{n|n-1} \\ where: \\ & f'(\hat x_{n-1})=A_x \\ & g'(\hat x_{n-1})=H_x \\ \end{aligned} EKF:prediction:update:where:x^n∣n−1=f(x^n−1)P^n∣n−1=AxP^n−1AxT+QKn=P^n∣n−1HxT(HxPnHxT+R)−1x^n=x^n∣n−1+Kk(z^n−g(x^n∣n−1))Pn=(E−KkHx)P^n∣n−1f′(x^n−1)=Axg′(x^n−1)=Hx
KF和EKF的预测和更新是一模一样的。可以根据卡尔曼增益的推导推出这个结论。
后记总结一下，扩展卡尔曼滤波EKF，实际上就是将非线性系统的状态和观测方程进行线性化后的线性卡尔曼滤波。
