通俗易懂地解释卡尔曼滤波(Python)（一）

预备知识

• 一维高斯函数

通常记作

• 高斯分布的加法公式：

• 高斯分布的乘法公式：

场景假设

一架飞机收到任务，需要前往一个指定好的目的地。假设飞机为直线运动，且飞机上配备有：

• GPS定位，通过卫星给出实时的位置信息（但有误差）

• IMU惯性测量传感器，能给出实时的速度信息（但有误差）

（ps：IMU中的加速度计模块可以测量加速度，根据已知初速度后计算速度，本身并不测量速度。

不考虑误差的情况下，飞机可以用单一传感器进行轨迹的计算，如仅利用GPS得到精确位置，或从初始位置开始利用IMU不断计算当前的精确位置和速度。但现实中两者都存在误差，为了降低误差的影响，需要对两个传感器进行融合处理，融合方式即为卡尔曼滤波。

如何用卡尔曼滤波来预测轨迹

所以当前时刻的已知条件如下：

• 飞机上一刻位置的概率分布N(pre_pos, pre_var)

• 此刻IMU测得速度的概率分布N(v, v_var)

• 此刻GPS测得位置的概率分布N(gps_pos, gps_var)

其中v_var和gps_var需要事先对传感器进行标定后得出（具体怎么做后面文章单独写，这里默认已知）

开始计算

• step1 利用IMU数据得到计算出的位置概率分布

用加法公式

• step2 将计算得到的位置与GPS测得的位置进行融合

用乘法公式

最终选择的位置为概率最大处的值，即

令

则上式变为

可以理解为两者用不同的权重进行融合，方差大的表示误差大，因为考虑使用小一点的权重。

而其中用来控制的权重系数K即为卡尔曼增益。

计算得到cur_pos和cur_var后，将其赋值给pre_pos和pre_var，重复上述步骤，即可计算下一时刻的位置。

Python实现

# -*- coding: utf-8 -*-
"""
Time:     2023/1/30 9:25
Author:   cjn
Version:  1.0.0
File:     kalman_filter.py
Describe: 
"""

import numpy as np

# 假设真实情况是加速度为0.25的运动
t = np.linspace(1, 200, 200)
truth_pos_array = (0.25 * t**2)/2

# 假设gps的方差为150，所以gps测量值为真实值加噪声值
gps_var = 200**2  # 探测位置传感器的方差
gps_pos_array = truth_pos_array + np.random.normal(0, 200, size=(t.shape[0]))
import matplotlib.pyplot as plt
plt.plot(t, truth_pos_array, label="truth position")
plt.plot(t, gps_pos_array, label="gps-measured position")


# 初始的估计导弹的位置就直接用GPS测量的位置
predict_pos_array = [gps_pos_array[0]]
predict_pos = gps_pos_array[0]

predict_var = 0  # 上一个预测位置的方差，这里定的是初始值，后续迭代
v_var = 160**2  # 速度传感器的方差

# 卡尔曼滤波函数
def kalman_filter(pre_pos, pre_var, v, v_var, gps_pos, gps_var):
    # 根据上一时刻位置和速度值，得到计算的下一时刻参数(x1, var)对应(u1, σ1平方)
    x1 = pre_pos + v
    var1 = pre_var + v_var

    # 根据上面算的x1和var1，以及探测位置传感器的x2和var2，得到预测该时刻位置的x和var
    # 先计算卡尔曼增益
    kalman_gain = var1 / (var1 + gps_var)
    predict = kalman_gain * x1+(1 - kalman_gain) * gps_pos
    predict_var = (var1 * gps_var) / (var1 + gps_var)
    return predict, predict_var


if __name__ == "__main__":
    for i in range(1, t.shape[0]):
        # 模拟从IMU读出的速度值
        v = (truth_pos_array[i] - truth_pos_array[i-1]) + np.random.normal(0, 160)
        predict_pos, predict_var = kalman_filter(predict_pos, predict_var, v, v_var, gps_pos_array[i], gps_var)
        predict_pos_array.append(predict_pos)

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

    plt.legend()
    plt.show()