做啥用的

我们想知道一个系统在某个时刻的真实状态，手段往往有多种，比如最常见的两种：

1. 用合适的传感器测量出来；
2. 根据上一时刻的状态用数学模型推算出来；

那么，应该相信哪一种办法得到的值呢？

怎么做的

先把上述概念实例化一下，对于一个小车，我们想知道它在当前时刻$t$距离障碍物的真实距离$x_t$。我们用两种办法：

1. 测距雷达测得的测量值$m_t$
2. 根据上一时刻的车速$v_{t-1}$和上一时刻的最终估计值$p_{t-1}$得到的预测值$n_t$，即有 $$n_t = p_{t-1}-v_{t-1}$$

然后，想办法平衡$m_t$和$n_t$，得到本时刻的最终估计值$p_t$。卡尔曼的做法是，将所有变量都视作一个正态的概率分布，而不是一个简单的标量值。即有

1. 自变量$v_{t-1}$ -> $v_{t-1}(\mu _v,\sigma _v)$，数据来自于传感器，可以认为均值和方差已知。
2. 自变量$m_{t}$ -> $m_{t}(\mu _m,\sigma _m)$，数据来自于传感器，可以认为均值和方差已知。
3. 中间变量$n_{t}$ -> $n_{t}(\mu _n,\sigma _n)$
4. 结果变量$p$ -> $p(\mu _p,\sigma _p)$

然后，按照经典方式，分两步计算来得到$p_t$

Step1 预测

首先因为

$$n_t = p_{t-1}-v_{t-1}$$

所以，可以得到$n_t$的均值和方差分别为

$$\mu_t^n = \mu_{t-1}^p - \mu_{t-1}^v$$ $$\sigma_t^n = \sigma_{t-1}^p + \sigma_{t-1}^v$$

Step2 融合/更新/校正

平衡$m_t$和$n_t$这两个概率分布的方式是概率的乘法，即有

$$\mu_t^p = \frac{\mu_t^n \sigma_t^m + \mu_t^m \sigma_t^n }{\sigma_t^m + \sigma_t^n }$$ $$\sigma_t^p = \frac{\sigma_t^m \sigma_t^n }{\sigma_t^m + \sigma_t^n }$$

由于正态分布在变量取均值是概率最大，所以$\mu_t^p$即为$t$时刻的最终估计值$p_t$。而$\sigma_t^p$则用于计算下时刻的最终估计值。

代码实例

import numpy as np
import matplotlib.pyplot as plt

t = np.linspace(1, 100, 100)  # 在1~100s内采样100次
a = 0.5  # 加速度值
position = (a * t ** 2) / 2

position_noise = position + np.random.normal(0, 120, size=(t.shape[0]))  # 模拟生成GPS位置测量数据（带噪声）

plt.plot(t, position, label='truth position')
plt.plot(t, position_noise, label='only use measured position')

# ---------------卡尔曼滤波----------------
# 初始的估计导弹的位置就直接用GPS测量的位置
predicts = [position_noise[0]]
position_predict = predicts[0]

predict_var = 0
odo_var = 120 ** 2  # 这是我们自己设定的位置测量仪器的方差，越大则测量值占比越低
v_std = 50  # 测量仪器的方差（这个方差在现实生活中是需要我们进行传感器标定才能算出来的，可搜Allan方差标定）
for i in range(1, t.shape[0]):
    dv = (position[i] - position[i - 1]) + np.random.normal(0, 50)  # 模拟从IMU读取出的速度
    position_predict = position_predict + dv  # 利用上个时刻的位置和速度预测当前位置
    predict_var += v_std ** 2  # 更新预测数据的方差
    # 下面是Kalman滤波
    position_predict = position_predict * odo_var / (predict_var + odo_var) + position_noise[i] * predict_var / (
                predict_var + odo_var)
    predict_var = (predict_var * odo_var) / (predict_var + odo_var) ** 2
    predicts.append(position_predict)

plt.plot(t, predicts, label='kalman filtered position')

plt.legend()
plt.show()

参考链接

1. https://zhuanlan.zhihu.com/p/77327349