MOT学习 - 卡尔曼滤波

参考资料：

• https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/
• 协方差矩阵：https://youzipi.blog.csdn.net/article/details/48788671
• https://www.bilibili.com/video/BV1Mu411B7Jy?p=1

协方差矩阵：对称矩阵，协方差表示的是两个随机变量的关系。

协方差表示在多大程序上x和y会共同变化。简单来说就是如果两个随机变量的协方差>0，则两者是正相关的，结果为负值就说明负相关的。如果为0，说明两者是不相关的

协方差矩阵的特征向量：第一特征向量是数据方差最大的方向，第二特征向量是与第一特征向量垂直的方向上数据方差最大的方向，第三特征向量是与第一和第二特征向量垂直的方向上数据方差最大的方向，以此类推。较长的u是第一特征向量，较短的v是第二特征向量。

What is it？

使用场景：对于某个动态系统有不确定的信息，需要对系统接下来要做什么做出有根据的猜测。卡尔曼滤波器非常适合不断变化的系统。比如：处理传感器噪声。 换句话说，我们的传感器至少有些不可靠，我们最初估计的每个状态都可能导致一系列传感器读数。
鲁棒性：即使混乱的现实出现并干扰了您猜测的清晰运动，卡尔曼滤波器通常也能很好地找出实际发生的情况。它可以利用您可能不会想到利用的疯狂现象之间的相关性！
实时性：优点是内存占用少（除了以前的状态之外不需要保留任何历史记录），而且速度非常快，非常适合实时问题和嵌入式系统。

机器人例子

一个机器人，状态量表示为：xk→=(p ,v ) ，包含position和velocity，然后外部条件有：

• 有 GPS sensor，但是精度只有10m；
• know something about how the robot moves：机器人运动朝向，电机转动步数（可以推测出机器人运动的距离），但是仍然有很多不可控的因素，比如：风的影响，轮子可能打滑。

结果：轮子转动的量可能不能准确地代表机器人实际走了多远，预测也不会完美。
场景特点总结：

• GPS 传感器告诉我们一些关于状态的信息，但只是间接的，并且带有一些不确定性或不准确性。
• 我们的预测告诉我们一些关于机器人如何移动的信息，但只是间接的，并且存在一些不确定性或不准确性。

卡尔曼滤波器要做的：利用所有可用的信息，得到一种比任何估计本身更好的答案。

卡尔曼滤波如何工作的
We don’t know what the actual position and velocity are; there are a whole range of possible combinations of position and velocity that might be true, but some of them are more likely than others:

卡尔曼滤波假设postion and velocity是随机，且都是高斯随机分布，每个变量有均值u和方差 σ2。
卡尔曼滤波的预测向量：Fk，该向量的作用是将k-1时刻状态分布移动到k时刻。

总
卡尔曼滤波所有公式

卡尔曼滤波简图

超参调整
需要调整的参数有四个：Q，R，P，初始值。
参考：

• Kalman Filter 卡尔曼滤波调参的实用方法和经验有哪些
• https://blog.csdn.net/callmeup/article/details/121489224

Q：代表的是系统估计的误差（方差），Q越大，表示系统的估计值越不可靠。
R：代表的是观测误差（方差），R越大，表示观测值越不可靠。
P：代表目前迭代值的误差（方差），P越大，也表示系统的估计值越不可靠。
初始值：系统估计初始值，P的初值决定了初始增益的大小，可以决定初始收敛速度。

Python样例

import matplotlib.pyplot as plt
import numpy as np

iters = 100

z = np.arange(1, iters + 1)  # observations. 观测值
noise = np.random.randn(iters)  # mean = 0, std = 0.1. 方差为1的高斯噪声
z = z + noise

x = np.ones(2)  # predicted states, position and velocity respectively. 状态(位移+速度), 初始为0, 0. 预测值速度越接近1越好
p = np.array(
    [[1, 0], [0, 1]],
)  # 状态协方差矩阵
f = np.array([[1, 1], [0, 1]])  # 状态转移矩阵

q = np.array(
    [[0.0001, 0], [0, 0.0001]],
)  # 预测噪声协方差矩阵

h = np.array([[1, 0]])  # 预测值 -> 观测值 的转换矩阵
r = np.array([1])  # 观测噪声方差 (观测值只有位移)

x_preds = []
for i in range(iters):
    # 更新预测 & 预测协方差
    x_ = np.matmul(f, x)
    p_ = np.matmul(np.matmul(f, p), f.T) + q
    # 得到卡尔曼系数
    k = np.matmul(p_, h.T) / (np.matmul(np.matmul(h, p_), h.T) + r)
    # 修正预测 & 更新预测协方差
    x = x_ + np.matmul(k, (np.array([z[i]]) - np.matmul(h, x_)))
    x_preds.append(x)
    p = (np.eye(2) - np.matmul(k, h)) * p_


x_preds = np.stack(x_preds)

plt.figure(figsize=(5, 5))
plt.grid(linestyle="-.", linewidth=1)

plt.plot(x_preds[:, 0], x_preds[:, 1], "red", linewidth=3)
plt.show()