算法 | 卡尔曼滤波器算法1——应用介绍（Matlab & simulink）

github：https://github.com/MichaelBeechan
CSDN：https://blog.csdn.net/u011344545

============================================================

其他相关卡尔曼滤波博客：
单片机开发 | 基于51单片机实现MPU6050的卡尔曼滤波算法（代码类）
单片机开发 | 基于MPU6050的卡尔曼滤波算法（代码类）

============================================================

1、前言（Introduction）

卡尔曼滤波是一种利用实测数据估计系统状态的算法。它最初是由匈牙利工程师鲁道夫·卡尔曼(Rudolf Kalman)开发的，卡尔曼滤波器算法就是以他的名字命名的。滤波器的算法分为两步：第一步预测系统状态，第二步利用噪声测量对系统状态估计进行细化。

现在有几个变种的卡尔曼滤波器。这些滤波器广泛用于依赖于估计的应用，包括计算机视觉、制导和导航系统、计量经济学和信号处理。

2、制导、导航和控制（Guidance, Navigation, and Control）

卡尔曼滤波器广泛应用于GNC系统中，如传感器融合，通过融合GPS和惯性测量单元(IMU)的测量来合成位置和速度信号。滤波器通常用于估计无法测量的信号的值，比如飞机发动机涡轮中的温度，在这种情况下，任何温度传感器都会失灵。在线性二次高斯控制中，滤波器还与线性二次调节器(LQR)补偿器一起使用。

下面利用卡尔曼滤波估计飞机的位置。

3、计算机视觉（Computer Vision）

在计算机视觉应用中，卡尔曼滤波器用于跟踪目标，预测目标的未来位置，考虑目标检测位置中的噪声，并帮助将多个目标与其对应的轨迹关联起来。

下面是跟踪球的轨迹。卡尔曼滤波的输出用红圈表示，目标检测用黑色表示。注意当球被遮挡，没有检测到;过滤器被用来预测它的位置。

4、基本卡尔曼滤波算法（Kalman Filtering）

研究说明了卡尔曼滤波器的设计与仿真。同时考虑了稳态和时变卡尔曼滤波器。

4.1 Plant Dynamics
考虑一个离散对象在输入 u[n] 上存在加性高斯噪声 w[n] ：

x[n+1] = Ax[n] + B(u[n] + w[n])
y[n] = Cx[n].

此外，设输出 y[n] 的噪声测量为vyv[n] ，v[n]表示测量噪声:

yv[n] = y[n] + v[n].

The following matrices represent the dynamics of the plant.

A = [1.1269   -0.4940    0.1129;
     1.0000         0         0;
          0    1.0000         0];

B = [-0.3832;
      0.5919;
      0.5191];

C = [1 0 0];

4.2 离散卡尔曼滤波器
给出了该问题的稳态卡尔曼滤波方程

上述方程中，
表示x[n]的估计，根据过去的测量结果给出
是基于上次测量的更新估计
根据当前估计存在：

在给定噪声协方差的情况下，选择创新增益M来最小化估计误差的稳态协方差：

您可以将时间和测量更新方程组合成一个状态空间模型，即卡尔曼滤波器：

滤波器产生记滤波器状态为：

参考文献：
Grimble, M.J., Robust Industrial Control: Optimal Design Approach for Polynomial Systems, Prentice Hall, 1994, p. 261 and pp. 443-456.