复习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)=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.

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

Fk=∂f∂X|X(k−1|k−1),uk

Hk=∂h∂X|X(k−1|k−1),uk