卡尔曼滤波的matlab实现

1. 概述

卡尔曼滤波（Kalman Filter）是一种利用线性系统状态方程，利用对系统的观测数据，对系统状态进行最优估计的算法。由于观测数据受到系统中的噪声和干扰的影响，所以系统状态的估计过程也可看作是滤波过程。应用场景之一有利用传感器跟踪感兴趣目标的位置，传感器获取的目标距离、速度、方位角等观测值往往含有噪声。卡尔曼滤波利用目标的动态信息与观测结果相结合，抑制噪声的影响，从而获得一个关于目标位置更准确的估计，这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。

1. 卡尔曼滤波原理

标准的卡尔曼滤波系统方程如下：

上面的两个式子分别叫作状态转移方程（1）和观测方程（2）。其中A叫作状态转移矩阵；B和uk是系统模型的参数；C叫作观测矩阵；sk是状态转移噪声向量或者叫过程噪声；vk是观测噪声向量；是k时刻的真实状态向量；是k时刻的预测值；zk是k时刻的观测向量。

卡尔曼滤波分为预测（Predict）和更新（Update）两个步骤：

预测：

                                     

更新：

                         

实现步骤如下：

第一步：根据公式（3）和公式（4）计算预测值以及预测值与真实值之间的协方差矩阵；（数据准备）

第二步：根据公式（5）和公式（6）计算卡尔曼增益，然后根据公式（7）估计；（滤波估计）

第三步：根据公式（8）计算估计值与真实值之间的误差协方差矩阵，用于下一次递推；（参数更新）

2、按照以上步骤经MATLAB仿真结果如下图1：

由图1可知，红色部分为滤波前，蓝色部分为滤波后，实现了较好的滤波效果。卡尔曼滤波算法的主要思想在于同时利用了目标的实时状态信息和观测结果，相比于直接利用单一观测估计系统状态而言，卡尔曼滤波算法利用了更多的先验信息，每一个时刻都对系统状态做估计，并预测了下一个时刻的系统大致状态，之后根据观测结果，修正预测量，从而完成对系统状态更准确的估计。其整个过程是递推进行的，每完成一次系统的状态估计，就计算一次估计值与真实值的误差协方差矩阵，用于下一次估计时的变量计算。

3、数据滤波是去除噪声还原真实数据的一种数据技术，卡尔曼滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。卡尔曼便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理。卡尔曼滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。