预备知识
一维高斯函数
通常记作
高斯分布的加法公式：
高斯分布的乘法公式：
场景假设
一架飞机收到任务，需要前往一个指定好的目的地。假设飞机为直线运动，且飞机上配备有：
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()
