1 问题
假设一物体以一初速度
v
0
v_0
v0位于一高度为
y
0
y_0
y0处正处于匀速下降状态,此时该物体启动制动装置,以一个加速度为
a
a
a的作用力反向运动
(1)建模
速度:
V
=
V
0
−
a
∗
t
V = V_0 - a*t
V=V0−a∗t
位置:
Y
=
−
V
∗
t
+
Y
0
−
1
2
a
∗
t
2
Y = -V*t + Y_0 - \frac{1}{2} a*t^2
Y=−V∗t+Y0−21a∗t2
(2)转换为状态空间方程
略
2 算法仿真
import numpy as np
import matplotlib.pyplot as plt
"""
速度:V = V0 - a*t
位置:Y = -V*t + Y0 - 1/2 a*t**2
"""
y0 = 100.0
DT = 0.1
g = 9.8
SIM_TIME = 100.0
U = 0.01
GPS_NOISE = np.diag([1, 1]) ** 2
A = np.array([[1.0, 0.0],
[-DT, 1.0]])
B = np.array([[DT],
[-DT**2]])
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,u):
#A = np.array([[1.0, 0.0],
# [-DT, 1.0]])
x = A.dot(x)-B.dot(u)
return x
def observation_model(x):
#H = np.array([[1.0, 0.0],
# [0.0, 1.0]])
z = H.dot(x)
return z
def observation(xtrue):
xTrue = motion_model(xtrue,U)
z = motion_model(xTrue,U) + GPS_NOISE @ np.random.randn(2, 1)
return xTrue,z
def kalman_filter(xEst, PEst, z):
# Predict
xPred = motion_model(xEst,U)
PPred = A @ PEst @ A.T
# Update
zPred = observation_model(xPred)
y = z - zPred
S = H @ PPred @ H.T + R
K = PPred @ H.T @ np.linalg.inv(S)
xEst = xPred + K @ y
PEst = (np.eye(len(xEst)) - K @ H) @ PPred
return xEst, PEst
time=0
x_array = []
y_array = []
z_array = []
k_array = []
v_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])
v_array.append(xEst[0])
x_array.append(time)
time += DT
plt.figure(0)
plt.plot(x_array,y_array,'g')
plt.plot(x_array,z_array,'r')
plt.plot(x_array,k_array,'b')
plt.show()
plt.figure(1)
plt.plot(x_array,v_array,'b')
plt.show()