龙猫的博客

后续在实际项目中实践算法的优劣。iEKF 简单理解就是。

卡尔曼滤波可以在线性模型，误差为高斯模型的情况下，对目标状态得出很好的估计效果，但如果系统存在非线性的因素，其效果就没有那么好了。其中，f 是非线性状态函数，h 是非线性量测函数，w 与 v 分别是高斯噪声，协方差矩阵为 Q 与 R，在上式中省略了。对于非线性系统的问题，比较常用的方法是将非线性的部分进行线性化处理，其中，EKF 就是对非线性系统进行线性化，即，将非线性函数，在上次估计值 x_hat(k-1) 处进行泰勒展开，其中，结合上面的非线性方程，之后将线性化的泰勒展开式代入上面的非线性方程。

在上期，使用一个简单的一维应用实例来加深了卡尔曼滤波的印象后，使用一个二维的例子来看一下卡尔曼的效果。将上式写作状态方程的形式，定义 x1 代表位移，x2 代表速度，x1_dot = x2 (x1的微分就是x2)，将x1，x2合并为一个二维的状态矩阵 x。其中，r 为测量噪声，传感器只能观测位移，不能观测速度。

在实际中，测量噪声 v 的方差是比较容易得到的，因为是传感器的方差，可以预先测量 n 组数据来计算出来，计算出的方差 R 与真实方差也是接近的，但过程噪声方差 Q 是比较难确定的，它是由建模引起的，需要实际判断与调试。推导了卡尔曼滤波的原理之后，使用一个简单的一维应用实例来训练一下，加深印象。对于此一维的系统，矩阵 A = 1，H = 1，B = 0。其中 w 为过程噪声，方差为Q， v 为测量噪声，方差为R。温度计的测量噪声 v 的方差为 R，假设为1.0^2。绿色为测量值，红色为估计值，蓝色为实际值。

1）当估计误差远大于测量误差时，k>>1，此时 x_hat(k) = x_hat(k-1) + z(k) - x_hat(k-1) = z(k)，此时就完全信任测量值。在上一小节中求解出了卡尔曼增益的表达式，但在表达式中的先验误差的协方差矩阵 P-(k) 的未知的，因此接下来就需要求解先验误差的协方差矩阵 P-(k)。结合上面的两个公式，对 w，v 来说，期望值都为0 ，也就是 E(w) = 0，E(v) = 0，则。其中 e(k) 表示估计误差，x(k) 表示真实值，x_hat(k) 表示后验估计值。