卡尔曼滤波算法

注：初识卡尔曼滤波，谨此记录
学习视频：b站【MATLAB中国】

1. 卡尔曼滤波器的用途

为什么使用卡尔曼滤波器？

• 卡尔曼滤波器是一种优化估计算法，可以预测感兴趣的参数（例如位置、速度、方向）
• 应用：制导与导航控制系统、计算机视觉系统以及信号处理

接下来举两个例子说明卡尔曼滤波器的常见用途：

用途1

eg1.【用途】 用来估算系统状态，当它无法被直接测量时

我们要出发去火星，如果我们的太空船的发动机能在足够高的温度下燃烧燃料，那么它产生的推力就足够让我们进入火星，但过高的温度会使发动机机械部件处于危险之中。
为了避免这一问题，我们需要密切关注燃烧室的内部温度Tin，但这并不容易，因为如果传感器放在发动机燃烧室内就会被熔化掉。因此，我们将传感器放在燃烧室附件温度较低的地方Text。

此时，我们无法直接得到内部温度Tin，只能测得附件外部温度Text。这时，我们就可以利用卡尔曼滤波器，利用间接测量值，计算内部温度的最优估算值（这样就能获取原本测不到的值，利用能测得的值）。

用途2

eg2. 如何使用卡尔曼滤波器来估计系统的状态，通过组合各种可能受噪音影响的数据源
我们需要去机场接机——使用汽车的导航系统，让我们看看车载传感器，那些得到当前位置并将我们导航到机场的传感器

• 惯性单元IMU：使用加速度计和陀螺仪来测量汽车的加速度和角速度
• 里程表：测量汽车行驶的相对距离
• GPS：接收来自卫星的信号，并定位汽车所在地球表面的位置。
  而在隧道中，很难利用GPS进行定位。为了更准确的获取汽车当前位置，可以使用IMU测量值和里程表读数

总而言之，传感器可以测量汽车的相对位置，可以快速更新但也容易漂移。GPS接收器提供绝对位置，但它更新没那么快，而且可能会有噪音。这时，我们就**可以使用卡尔曼滤波器结合三个测量值，得到汽车最合适的估算位置。

★总结

1. 卡尔曼滤波器用于优化估算我们感兴趣的量
2. 用途1：当这些量无法直接测量，但可以间接测量的时候
3. 用途2：用于估算系统状态，通过组合各种可能受噪音影响的传感器测量值

2. 卡尔曼滤波器是什么

卡尔曼滤波器是一种设计最优状态观测器的方法。

状态观测器

状态观察可以帮助我们估计无法直接查看或测量的内容。

使用之前飞船燃料室的内部温度的例子，可以使用燃油流量和测量值来估算其内部温度。那么如何估算内部温度呢？
将燃油量值输入我们的数学模型，将会得到估算的输出。但是估算的外部温度并不能吻合测量的温度，这就是为什么要使用状态估算器来估算内部状态。

状态估算器的工作原理：

• 目标是估计的外部温度与测量的外部温度能吻合（如果这两者相等，说明表示模型收敛到了真实系统），那么估算的内部温度也能达到收敛到真实内部温度）。我们需要做的就是尽量减少估算和测量的外部温度之间的差异
• 通过状态器及其控制器K形成的闭环，尝试消除估算和测量外部温度之间的误差
• 总而言之，无法直接测量内部发动机温度，但是知道火箭的燃油量，我们可以运行数学模型，估算输出，再结合实际测量值来估计系统的内部状态

以数学的方式解释状态观测器
输入为u，输出为y，我们想要估计的状态为x。我们的目标是x^的估计值能逼近x，将它们之间的差值定义为误差ebos。
写出系统和观测器的方程式，相减可得误差方程。最后得到误差方程是个指数函数
**注：**给观测器建立反馈回路的重要性在于，可以控制误差函数的衰减率，通过选择相应的控制器增益K

3. 卡尔曼滤波器如何工作

对于一组数据而言，可以用均值和方差来判断它的好坏

ed. 汽车的输入是油门，输出是汽车位置。该系统只有一个状态——汽车位置，我们正在测量这个状态，因此矩阵C=1。我们需要尽可能准确地得到y。
GPS噪音读数会有很多噪音，使用v表示这种测量噪音，是一个随机变量。此外还有过程噪音w，可以表示风的影响或汽车速度的变化。虽然这些噪音不遵循模式，但可以使用概率论描述它们的平均属性。设v~N(0,R) ，W~N(0,Q)