卡尔曼滤波怎么做的？

  2. 根据上一秒导弹的位置 和 导弹的的速度估计出当前时刻导弹的位置粗略估计值。
  3. 将雷达测得导弹位置测量值和我们计算出的导弹位置粗略估计值根据这两种数据可信度来进行线性加权和得到准确的导弹位置估计值。

在前面我们也提到了导弹的位置和雷达测量值都是有误差的。所以卡尔曼想用概率来衡量数据的可信度。

比如：雷达测量的数据它就不只是一个数字了。而是说测量发现导弹有0.8的概率在7m那个位置，有0.1的概率在7.2m那个位置。有0.1的概率在6.9m那个位置。这些数据就叫做概率分布。概率分布的意思就是很多个值还有它们各自出现的概率多大所组成的数据就叫做概率分布。

卡尔曼认为导弹速度、导弹位置、雷达测距的测量值这些都服从正态分布（对就是高中学的那个正态分布）。这是啥意思呢？比如下面这个图是讲上个时刻导弹的位置的概率分布，从图中看出它在10m（横轴）那个位置的纵坐标最高所以概率最大。它在其他地方的概率相对小一点。

卡尔曼_数据

在前面我们知道了卡尔曼滤波算法为了融合雷达测量值和上个时刻的导弹状态数据来减少误差，需要实施“两步走”战略。

  2. 根据上一秒导弹的位置 和 导弹的速度估计出当前时刻导弹的位置粗略估计值。
  3. 将雷达测得导弹位置测量值和我们计算出的导弹位置粗略估计值根据这两种数据可信度来进行线性加权和得到准确的导弹位置估计值。

卡尔曼滤波Python代码实践

这个实践的已知量是导弹做水平运动，已知导弹每个时刻的速度dv,和速度测量仪器的标准差是v_std，还已知每个时刻用GPS测量出导弹位置position_noise以及GPS的方差是predict_var。（注意标准差的平方是方差，不用觉得奇怪）我们需要用卡尔曼滤波根据这些信息获得融合两种传感器后的位置信息position_predict卡尔曼_概率分布_02

 

 

# -*- coding: utf-8 -*-
"""
@司南牧 
"""
import numpy as np


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位置测量数据（带噪声）
import matplotlib.pyplot as plt
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()