浅析经典Kalman滤波

01 卡尔曼滤波是什么？

Kalman滤波是在时域上运用状态空间，递推得到的一种滤波算法，便于在计算机上实时实现，计算量和存储量小。该方法可处理多变量非平稳随机过程滤波问题，可处理时变系统滤波问题。例如飞机在飞行过程中，遇到的干扰通常是时变非平稳的噪声，此时运用卡尔曼滤波可有效去除干扰，得到较真实的状态估计数据。

02 卡尔曼滤波在飞控系统中的作用？

飞机在飞行过程中，其飞行的俯仰横滚角度与航向角度通常磁力计与陀螺仪提供，飞机的位置与速度通常由加速度计通过机理建模得到，简单来说飞机的角度与位置速度等数据可以由传感器和飞控机理算法得到，但是由于飞机在飞行过程中会有噪声输入，此时状态方程与观测方程会有一定的噪声输入，那么反馈的数据就是模糊不准确的，如何去除掉这些外来的干扰噪声，让飞机得到真实的飞行数据呢？那么卡尔曼滤波就可以实现这样的功能。

03 举例说明：

如图（1）所示，飞机在飞行过程中有外来噪声输入，传感器采集到有干扰的数据送给滤波估计器，滤波器负责去除这些噪声还原飞机真实的数据送给控制器，控制器得到飞机的真实数据，控制输出到飞机。这样飞机有了真实的数据，就可以更好的控制飞机，达到稳定飞行的目的。

卡尔曼滤波公式

Q与R为w(t),v(t)的方差阵；P为状态估计方差阵。

注：所引用公式出自黑龙江大学邓自立教授所著《建模与估计》p126-p129。

卡尔曼滤波简化

将卡尔曼滤波公式（2）的前三步进行简化合并，得到如下公式：

卡尔曼滤波递推算法框图

卡尔曼滤波递推算法框图简化

Matlab中卡尔曼滤波实现

以下初步讲解卡尔曼滤波在Matlab中仿真实现，所用资料均来自阿木实验室-铂贝学院出品的《卡尔曼滤波（理论）》第一节课，有兴趣的读者可以一步观看视频讲解，第一节课为试听免费课程。（铂贝学院：https://bbs.amovlab.com/plugin.php?id=zhanmishu_video:video&mod=video&cid=19）

◆如图（2）图（3）所示，以小车的位置速度为状态估计目标，由位置与速度的物理机体关系，对小车建模，得到状态估计方程；对小车的传感器建模，得到观测方程。