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

1 问题

假设一物体以一初速度$v_0$位于一高度为$y_0$处正处于匀速下降状态，此时该物体启动制动装置，以一个加速度为$a$的作用力反向运动
（1）建模
速度：$V=V_0-a\ast t$
位置：$Y=-V\ast t+Y_0-\frac{1}{2}a\ast t^2$
（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() 
 

3 仿真结果