无人机常用算法——卡尔曼滤波器（七）

sigma

无人机常用算法——卡尔曼滤波器（七） [复制链接]

三、扩展卡尔曼滤波

卡尔曼滤波是在线性高斯情况下利用最小均方误差准则获得目标的动态估计，适应于过程和测量都属于线性系统，且误差符合高斯分布的系统。但是实际上很多系统都存在一定的非线性，表现在过程方程（状态方程）是非线性的，或者观测与状态之间的关系（测量方程）是非线性的。这种情况下就不能使用一般的卡尔曼滤波了。解决的方法是将非线性关系进行线性近似，进而转化成线性问题。最常用的线性化方法是泰勒级数展开，忽略高次项。在此基础上的卡尔曼滤波就是扩展卡尔曼滤波（EKF）。泰勒展开一般可以取一次项或者二次项（忽略三次以后的高次项）。仿真结果表明，二阶扩展卡尔曼滤波的性能远比一阶的好，而二阶以上的扩展卡尔曼滤波性能与二阶相比没有明显提高，所以一般不采用。是二阶的计算量很大，所以一般采用一阶卡尔曼滤波。

由于状态方程或测量方程可能是非线性的，所以不能写成标准的线性化的格式，所以记成如下的表达形式，当然对于实际的系统就需要写出真实的状态方程和测量方程表达式，而不需要整理成标准的线性化格式。

下面给出经过一阶泰勒展开线性化的卡尔曼滤波递推公式。

状态方程：（直接写成函数的形式）

x(k) = f (x(k −1),u(k −1),w(k −1))

测量方程：

z(k) = h(x(k),v(k))

3.1 扩展卡尔曼滤波的公式为

Q(k −1) 是噪声 w(k −1) 的协方差，

R(k)是噪声v(k)的协方差。

月下良缘

谢谢分享！