kf 滤波matlab程序,MATLAB实现卡尔曼滤波器(KF、EKF)

卡尔曼滤波器

卡尔曼滤波器是一个 ”optimal recursive data processing algorithm”(最优化自回归数据处理算法)。

先说一个例子：

假设我们要研究的对象是一个房间的温度。

1.根据经验，温度是恒定的，即上一分钟的温度等于现在这一分钟的温度，经验即预测，但这并不是完全可信的，即存在一定的误差。我们假设成高斯白噪声。

2.另外在房间里放一个温度计，实时检测房间的温度，这是观测值。同样地也存在误差，也是高斯白噪声。

我们现在的目标就是，用这两个值，结合它们各自的噪声，估算出房间的实际温度。假设要估算k时刻的实际温度值，首先需要知道k-1时刻的温度值。

1)预测值，假设k-1时刻房间温度为23度，则k时刻的温度应该也为23度(恒定)。同时该值的偏差为5(5是3的平方加上4的平方再开方，其中3为k-1时刻估算出的最优温度值的偏差3，4为对预测的偏差)。

2)测量值，假设为25度，同时偏差为4度。

那么实际值为多少？相信谁多一点，可用它们的协方差来判断：

，Kg = 0.78

则实际的温度值是：23+0.78*(25-23) = 24.56度。可以看出温度计测量的covariance较小，所以实际值比较偏向温度计。

在进入k+1时刻估算前，需要算出k时刻最优值(24.56)的偏差。算法，5即k时刻估算所用k-1时刻23度温度值的偏差。得出的2.35即k时刻最优值的偏差(对应上面k-1时刻的3)。

这样卡尔曼滤波器就不断得把covariance递归，从而估算出最优值。而且它只保留上一刻的covariance。上面的Kg就是卡尔曼增益(Kalman Gain)。

真正工程系统上的卡尔曼滤波器算法

首先引入一个离散控制过程的系统，改系统可用一个线性随机微分方程(linear stochastic difference equation)来描述：

再加上系统的测量值：

其中：X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模系统，它们为矩阵。

Z(k)是k时刻的测量值，H是测量参数，对于多模系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声，它们被假设成高斯白噪声，它们的covariance分别是Q，R。

对于满足上面条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。

卡尔曼滤波器算法流程及核心公式

首先利用系统过程模型，来预测下一个状态的系统ÿ