复习EKF

复习EKF

---

扩展卡尔曼滤波是在卡尔曼滤波的基础上进行泰勒展开实现的

卡尔曼滤波的五个基础方程： 

• 系统状态方程（由上一状态滤波值预测当前状态估计值）：
  $$X(k|k-1)=AX(k-1|k-1)+BU(k)+W(k)$$
• 预测方程不确定性更新(Q表示w(k)的协方差矩阵，不随系统状态变化而比变化)：

$$P(k|k-1)=AP(k-1|k-1)A'+Q$$

形象的理解：上一次预测值与滤波值的偏差和高斯噪声的叠加。协方差矩阵表示的是矩阵各个位置之间的方差量，所以用APA’的形式计算

• 最优化估计状态方程：

$$X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))$$

• 卡尔曼增益计算方程（通过对比观测值与预测值方差的大小计算增益,R表示观测方程的噪声协方差矩阵）：

$$Kg(k)=\frac{P(k|k-1)H'}{HP(k|k-1)H'+R}$$

• 更新协方差矩阵：

$$P(k|k)=(I-Kg(k)H)P(k|k-1)$$

实际使用中，需要我们设置观测方程和预测方程的协方差矩阵

---

扩展卡尔曼滤波

卡尔曼滤波针对的是线性系统的滤波方法，在实际应用中，绝大部分系统是非线性的。为了解决非线性的滤波问题，需要使用扩展卡尔曼滤波（EKF） 

实现原理;在估计状态点，对观测方程和状态方程进行线性化处理。一般采用泰勒展开保留其一阶项。

缺点：1.由于只保留一阶项，所以扩展卡尔曼滤波只能达到二阶精度，这就要求系统为弱线性化的。如果系统非线性状态较为强烈，可考虑使用无迹卡尔曼滤波（UKF）;2.

实现方法： 雅可比矩阵更新模型

$$F_k=\frac{\partial f}{\partial X}|_{X(k-1|k-1),u_k}$$
$$H_k=\frac{\partial h}{\partial X}|_{X(k-1|k-1),u_k}$$