卡尔曼滤波算法Python代码实践

东方耀

卡尔曼滤波算法Python代码实践
卡尔曼认为导弹速度、导弹位置、雷达测距的测量值（误差）这些都服从正态分布 随机变量
卡尔曼滤波算法为了融合雷达测量值和上个时刻的导弹状态数据来减少误差
“两步走”战略:
1、根据上一秒导弹的位置 和 导弹的速度估计出当前时刻导弹的位置粗略估计值
2、将雷达测得导弹位置测量值和我们计算出的导弹位置粗略估计值根据这两种数据可信度来进行线性加权和得到准确的导弹位置估计值


计算粗略估计值（这个是做了一个硬假设：导弹是保持匀速运动

1. # -*- coding: utf-8 -*-
   
2. """
   
3. https://zhuanlan.zhihu.com/p/77327349    参考
   
4. 卡尔曼滤波算法Python代码实践
   
5. 卡尔曼认为导弹速度、导弹位置、雷达测距的测量值（误差）这些都服从正态分布   随机变量
   
6. 卡尔曼滤波算法为了融合雷达测量值和上个时刻的导弹状态数据来减少误差
   
7. “两步走”战略:
   
8. 1、根据上一秒导弹的位置 和 导弹的速度估计出当前时刻导弹的位置粗略估计值
   
9. 2、将雷达测得导弹位置测量值和我们计算出的导弹位置粗略估计值根据这两种数据可信度来进行线性加权和得到准确的导弹位置估计值
   
10. 
    
11. 计算粗略估计值（这个是做了一个硬假设：导弹是保持匀速运动
    
12. """
    
13. import numpy as np
    
14. 
    
15. # 模拟数据
    
16. t = np.linspace(1, 100, 100)
    
17. a = 0.5
    
18. # 抛物线：模拟真实导弹运动轨迹
    
19. position = (a * t ** 2) / 2
    
20. 
    
21. # 雷达观测值 加上了雷达的噪声误差  均值为0 标准差为std=120
    
22. position_noise_radar = position + np.random.normal(0, 120, size=(t.shape[0]))
    
23. import matplotlib.pyplot as plt
    
24. 
    
25. plt.plot(t, position, label='truth distance', linestyle='-', color='black', linewidth=4)
    
26. plt.plot(t, position_noise_radar, label='radar measured distance', color='blue')
    
27. 
    
28. # 初始的估计导弹的位置就直接用雷达测量的位置
    
29. # 开始卡尔曼滤波去预测，需要第一个时刻的 距离（第一个没办法，直接用 雷达的观测值） 只能完全相信仪器的
    
30. predicts = [position_noise_radar[0]]   # 卡尔曼的预测值 列表 为了画图
    
31. position_predict = predicts[0]   # 每一个时刻的 卡尔曼预测值 单个
    
32. 
    
33. predict_var = 0   # 估计值的方差
    
34. radar_var = 120 ** 2  # 雷达观测的方差，越大则测量值占比越低  var是方差  std是标准差
    
35. v_std = 50  # 测量速度仪器的标准差
    
36. 
    
37. # 从1开始：不是从0开始   正态分布 均值与方差都需要计算
    
38. for i in range(1, t.shape[0]):
    
39.     # 仪器读取的速度 = 真实的速度 + 误差(测量速度仪器的)
    
40.     dv = (position[i] - position[i - 1]) + np.random.normal(0, v_std)  # 模拟从IMU读取出的速度
    
41.     # 这里是计算：粗略估计值的均值(上一个时刻精确估计值 + 速度*时间=当前时刻的粗略估计值的均值)
    
42.     position_predict = position_predict + dv*1  # 利用上个时刻的位置和速度预测当前位置
    
43.     # 上一个时刻精确估计值的方差 +  速度的方差 = 当前时刻 粗略估计值的方差
    
44.     predict_var += v_std ** 2  # 更新方差
    
45.     # 下面是Kalman滤波  有个公式：方差为权重
    
46. 
    
47.     # 当前时刻的精确预测(计算均值 概率最大的地方)  观测值（雷达）的方差 + 粗略估计值的方差
    
48.     # 观测值与当前时刻粗略估计值的融合 = 当前时刻的精确 估计值
    
49.     # 根据这两种数据可信度来进行线性加权和得到,当然加权和的权重之和应该等于1,所以除以方差之和
    
50.     position_predict = position_predict * radar_var / (predict_var + radar_var) + position_noise_radar[i] * predict_var / (
    
51.             predict_var + radar_var)
    
52.     # 当前时刻的精确估计值的方差predict_var  (方差之积)除以(方差之和的平方)
    
53.     predict_var = (predict_var * radar_var) / (predict_var + radar_var) ** 2
    
54.     # 当前时刻的精确 估计值 的均值position_predict
    
55.     predicts.append(position_predict)
    
56. 
    
57. 
    
58. plt.plot(t, predicts, label='kalman filtered distance', color='red')
    
59. plt.xlabel("time(s)")
    
60. plt.ylabel("distance(m)")
    
61. plt.legend()
    
62. # plt.savefig('calman.png', dpi=200)
    
63. plt.show()
    

复制代码