作者 | 南叔先生

原文地址： http://t.csdn.cn/s8RaT

01 Kalman用于解决什么的问题

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

人话就是： 线性数学模型算出预测值+传感测量值=更准确的测量值。

02 先来看一下姿态估计问题

03 看几个例子

3.1 例题1

假设有一个标量系统，信号与观测模型为：

其中 a 为常数， n[k] 和 w[k] 是不相关的零均值白噪声，方差分别为 \sigma^2_n 和 \sigma^2 。系统的起始变量 x[0] 为随机变量，其均值为零，方差为 P_x[0] 。

（1）求估计 x[k] 的卡尔曼滤波算法；

（2）当 a=0.9,\sigma^2_n=1,\sigma^2=10,P_x[0]=10 时的卡尔曼滤波增益和滤波误差方差。

根据卡尔曼算法，预测方程为：

预测误差方差为：

卡尔曼增益为：

滤波方程：

滤波误差方差

起始：