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],[0.0, 1.0]])Q = np.diag([1.0, 1.0]) ** 2  
R = np.diag([1.0, 1.0]) ** 2  def motion_model(x):#A = np.array([[1.0, 0.0],#              [-DT, 1.0]])x = A.dot(x)return xdef observation_model(x):#H = np.array([[1.0, 0.0],#             [0.0, 1.0]])z = H.dot(x)return zdef observation(xtrue):xTrue = motion_model(xtrue)z = motion_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1)return xTrue,zdef kalman_filter(xEst, PEst, z):#  Predict xPred =  motion_model(xEst)PPred = A @ PEst @ A.T#  UpdatezPred = observation_model(xPred)y = z - zPredS = H @ PPred @ H.T + RK = PPred @ H.T @ np.linalg.inv(S)xEst = xPred + K @ yPEst = (np.eye(len(xEst)) - K @ H) @ PPredreturn xEst, PEsttime=0x_array = []    
y_array = []
z_array = []
k_array = []xEst = np.zeros((2, 1))
PEst = np.eye(2)
xTrue = np.array([[g*DT],[y0]])
xEst[0]=g*DT 
xEst[1]=y0   
while SIM_TIME >= time:xTrue, z = observation(xTrue)xEst, PEst = kalman_filter(xEst, PEst,z)z_array.append(z[1])y_array.append(xTrue[1])k_array.append(xEst[1])x_array.append(time)time += DTplt.plot(x_array,y_array,'g')
plt.plot(x_array,z_array,'r')
plt.plot(x_array,k_array,'b')
plt.show() 

3 实验结果