卡尔曼滤波Kalman Filter零基础入门到实践（上部）

参考视频：入门（秒懂滤波概要）_哔哩哔哩_bilibili

一、入门

1.引入

假设超声波距离传感器每1ms给单片机发数据。

理论数据为黑点， 测量数据曲线为红线，引入滤波后的数据为紫线

引入滤波的作用是过滤数据中的噪声，使信号更趋于真实值。

2.卡尔曼滤波适用系统

卡尔曼滤波适用于线性高斯系统

2.1线性的理解

线性满足叠加性与齐次性。

如何理解叠加性？

如何理解齐次性？

输入x增大k倍，输出y也增大k倍

2.2高斯的理解

高斯：噪声满足正态分布

3.宏观意义：滤波即加权

理想状态：信号*1 +噪声*0（数字表示权值）

低通滤波：低频信号*1 + 高频信号*0

卡尔曼滤波：估计值*（待选择）+观测值*（待选择）；选择估计值与观测值的权重，总体得到一个最佳修正值。

二、进阶（基本滤波知识）

1.状态空间表达式（便于之后卡尔曼滤波公式的理解）

状态方程

其中，是当前状态的当前值；是上一个时刻该状态的值；是检测输入；是过程噪声；A是一个状态转移矩阵（用于刻画系统从一个状态转移到另一个状态的概率）；B是一个控制矩阵。

观测方程

其中，是要观测的值；是观测噪声（与观测器的误差有关）；

举例

 加热一壶水，温度计观测到的温度与水温状态的关系C=1，当前温度计测量的温度=当前状态的当前水温+温度计的误差，A取单位矩阵，为每一时刻会增加多少度，是过程噪声（比如加热过程中环境温度的影响），观测方程为

状态方程为

2.高斯分布

2.1直观图解

在二维和三维中，不管这些点投影到x轴、y轴还是z轴，都是正态分布

2.2参数分析

2.2.1 和

：符合正态分布，均值为0，方差为

：符合正态分布，均值为0，方差为

统称以上两个为高斯白噪声

举例

假设车的位置移动了1000m，GPS检测值为，方差为1m噪声

则=，=1m

假设车速是3000m/s，在风速的作用下，GPS的检测值为

则= m/s，n服从正态分布，方差=1m/s

2.2.2 方差

二维协方差（多维也类似）

3.超参数

Q、R、N  PID

之后主要调的是Q和R（上面提到的方差）

4.卡尔曼直观图解

x轴是位置，y轴是概率密度，是后验估计值，也叫最优估计值（修正值），是先验估计值，是观测值，是当前的最优估计值

：卡尔曼滤波最终输出的值

：根据估计出来的一个当前的估计值

：是由先验估计值和当前观测值取共有部分得到的