参考文档：
http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures
https://blog.csdn.net/AdamShan/article/details/78248421
https://blog.csdn.net/u011362822/article/details/95905113
卡尔曼滤波的适用场景、原理以及使用中的关键概念
- 在控制领域，获取控制对象精确的当前状态至关重要。但是在传感器测量过程中由于自身误差和外部干扰导致采样值不准确，卡尔曼滤波的作用就是为了纠正(correct)这些不准确，从而得到较准确的“状态值”(采样数据)；
- 原理大概是这样的：
a. 卡尔曼滤波假设各种采样数据的噪音（测量值和实际值的差）符合高斯分布，并且这些采样数据之间存在某些关系；
b. 例如，有两种采样数据分别是小车的速度(v)和离障碍物的距离(d)（关系：d2=d1-△t*v，d1/d2 分别是相隔一段时间的距离采样值，采样相隔时间为 △t）；
c. 如下图所示，采样数据 v 和 d 的噪音分别对应高斯分布(μ0, σ0),(μ1, σ1)，μ0 和 μ1 分别代表出现概率最高的噪音值；
d. 卡尔曼滤波，参照高斯分布的乘积仍然符合高斯分布原理，根据速度(v)和离障碍物的距离(d)采样值、变化值以及关系公式，计算出一组权重公式（卡尔曼增益矩阵），对采样结果进行补偿，从而降低噪音，如上图，两个高斯分布的乘积形成的新的高斯分布，噪音的最大振幅降低了。
e. 卡尔曼滤波是一种迭代滤波器，使用不断迭代“训练”出的“规律”，对当前采样值的进行优化。 - 想使用卡尔曼滤波，需要具备下列条件：
a. 多种 “状态值”（不只一个），并且它们之间存在可以用公式描述的关系；
b. 这些数据都是独立传感器的采样结果；
c. 可以获得每次采样的间隔时间； - 使用卡尔曼滤波中相关的概念：
a. 状态协方差矩阵 Pk ：描述多“状态值”之间互相影响程度的矩阵，初始值随意，几次迭代后会逼近状态值之间的真实关系；
b. 预测矩阵 Fk ：描述各种“状态值”伴随时间的变化规律，包含了各种“状态值” S(k-1) 计算下一个“状态值” Sk 的计算公式，示意图如下。
c. 控制矩阵 Bk 和控制向量 μ →：描述控制指令引发“状态值”变化规律的矩阵和参数。比如，无人车内部有操控指令（加速、减速、左转、右转等），操控指令对无人车状态带来的变化时可以推断的，例如控制无人车加速，则该矩阵代表此加速度对速度的影响，为可选项。
d. 外部噪音协方矩阵 Qk ：代表可能存在的外部噪音，比如风速、地势等，为可选项。
e. 单位变换矩阵 Hk ： 预测值和传感器读值的单位和比例可能有区别，这个矩阵用于两个矩阵进行计算前必须的单位和比例的转换，是个常量。
f. 传感器噪音协方矩阵 Rk ：代表传感器本身的误差噪音，Rk 的中心值( z →)等于传感器本次的测量值，Rk 由传感器厂家提供。
g. 卡尔曼增益矩阵 K : 卡尔曼滤波的核心，示意图如下，本质上是一个融合预测值和测量值的一个权重矩阵，融合的过程就是用“预测值”去纠正“测量值”的过程。
h. 迭代更新的计算流程图
实践场景
- 霍尔传感器测速
a. 硬件配置：玩具车轮上安装 3 个磁铁，搭配一个霍尔传感器
b. 基本原理：使用 STM32 定时器获得进出磁场的时间戳
c. 定时器累加频率，20KHz - 超声波传感器测距
测距实验数据：接近障碍物时采样值偏大，会引起避障不及时
| 实际距离 cm | 测量距离 cm |
|---|---|
| 12 | 21 |
| 20 | 28 |
| 41.5 | 49 |
| 60 | 65 |
| 121.5 | 127 |
| 161.5 | 149-152 |
| 201.5 | 186-188 |
| 281.5 | 232-234 |
| 361.5 | 310-327 |
- 为什么需要卡尔曼滤波：超声波传感器受环境影响太大，尤其是接近障碍物时采样值偏大，希望通过滤波纠正。
套用卡尔曼滤波公式
由于 csdn 的 markdown 中的数学公式并不不好用，下面内容是 word 中编辑好的截图。
实现技巧：利用 matlab 把二维矩阵运算转换为符号运算
由于 STM32 单片机中存储空间有限，计算能力有限，调用矩阵计算函数库不现实，所以可以利用 matlab 的符号定义功能，把矩阵运算转换为符号运算。
% 把二维的卡尔曼滤波的矩阵运算转换为普通运算
clear
clc
% 符号定义
syms p11 p12 p21 p22 vk dk ve de delt_t
% 初始状态
P_km1 = [p11, p12; p21, p22]
x_km1 = [ve; de]
% 预测矩阵
F = [1, 0; -delt_t/1000, 1]
Ft = [1, -delt_t/1000; 0, 1]
% 运动噪音协方差矩阵
cor_a = 10000
Q = [(delt_t/1000)^2, 1/2*(delt_t/1000)^3; 1/2*(delt_t/1000)^3, 1/4*(delt_t/1000)^4]*cor_a
% 预测状态
x_exp_k = F*x_km1
P_k = F*P_km1*Ft + Q
% 测量状态
z_k = [vk; dk]
% 测量噪音
cor_ve = 0.000625
cor_de = 0.0025
% R_k = [cor_ve*vk^2, 0; 0, cor_de*dk^2]
R_k = [cor_ve*vk^2, 0; 0, (150/dk)^2]
%卡尔曼增益
K = P_k/(P_k + R_k)
%更新状态
P_k_target = P_k - K*P_k
x_exp_k_target = x_exp_k + K*(z_k - x_exp_k)
实验结果
实验调优反复进行了很多次，只只最后一次，已经有一定的效果，但是还需要继续调试。
- 遇到问题：障碍物变动或者运动方向变化会使预测失效，导致结果误差偏大；解决思路：加入控制信息，发生运动变化时，重新计算。
- 采样误差过大：传感器超过 2m 后的测试结果极不稳定，不信任 2m 以上的结果，2m 作为超声波传感器的极限距离，所有大于 2m 的测量值一律返回2m。
- 调试和优化的思路是：信任速度采样结果，而怀疑距离采样结果，最终结果如下（红线为纠正后的结果，蓝线为采样数据）：
