通俗易懂地解释卡尔曼滤波(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()
通俗易懂地解释卡尔曼滤波(Python)(一)_第1张图片

你可能感兴趣的:(机器人,python)