卡尔曼滤波实例分析（一）

原创

已于 2023-07-06 09:26:09 修改 · 1.3k 阅读

3 ·

CC 4.0 BY-SA版权

文章标签：

#算法

于 2023-07-05 20:00:34 首次发布

该文通过Python实现了一个自由落体物体下落高度的测量模型，利用卡尔曼滤波算法处理由特殊雷达获取的带有噪声的数据。模型考虑了重力加速度，通过状态空间方程描述物体运动，并通过滤波器优化估计结果，对比了真实值、观测值和滤波后估计值。

1 现实问题

假设一个物体位于1000米处以自由落体运动，地面有一台具有特殊功能的雷达，对其进行观察，现需要对其下落的高度进行测量；
（1）建模
速度：V = gt
位置：Y = -Vt + Y0
（2）转化为状态空间方程
略

2 算法实现

import numpy as np
import matplotlib.pyplot as plt
"""
速度：V = g*t 
位置：Y = -V*t + Y0

"""
y0 = 1000.0
DT = 0.1
g = 9.8
SIM_TIME = 50.0  

GPS_NOISE = np.diag([1, 1]) ** 2
A = np.array([[1.0, 0.0],
             [-DT, 1.0]])

H = np.array([[1.0, 0.0],
              <