卡尔曼滤波— Constant Velocity Model

假设你开车进入隧道，GPS信号丢失，现在我们要确定汽车在隧道内的位置。汽车的绝对速度可以通过车轮转速计算得到，汽车朝向可以通过yaw rate sensor(A yaw-rate sensor is a gyroscopic device that measures a vehicle’s angular velocity around its vertical axis. )得到，因此可以获得X轴和Y轴速度分量Vx，Vy

首先确定状态变量，恒速度模型中取状态变量为汽车位置和速度：

根据运动学定律(The basic idea of any motion models is that a mass cannot move arbitrarily due to inertia)：

由于GPS信号丢失，不能直接测量汽车位置，则观测模型为：

卡尔曼滤波步骤如下图所示：

# -*- coding: utf-8 -*-
import numpy as np
import matplotlib.pyplot as plt

# Initial State x0
x = np.matrix([[0.0, 0.0, 0.0, 0.0]]).T

# Initial Uncertainty P0
P = np.diag([1000.0, 1000.0, 1000.0, 1000.0])

dt = 0.1 # Time Step between Filter Steps

# Dynamic Matrix A
A = np.matrix([[1.0, 0.0, dt, 0.0],
              [0.0, 1.0, 0.0, dt],
              [0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 1.0]])

# Measurement Matrix
# We directly measure the velocity vx and vy
H = np.matrix([[0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 1.0]])

# Measurement Noise Covariance
ra = 10.0**2
R = np.matrix([[ra, 0.0],
              [0.0, ra]])

# Process Noise Covariance
# The Position of the car can be influenced by a force (e.g. wind), which leads
# to an acceleration disturbance (noise). This process noise has to be modeled
# with the process noise covariance matrix Q.
sv = 8.8
G = np.matrix([[0.5*dt**2],
               [0.5*dt**2],
               [dt],
               [dt]])
Q = G*G.T*sv**2

I = np.eye(4)

# Measurement
m = 200 # 200个测量点
vx= 20  # in X
vy= 10  # in Y
mx = np.array(vx+np.random.randn(m))
my = np.array(vy+np.random.randn(m))
measurements = np.vstack((mx,my))

# Preallocation for Plotting
xt = []
yt = []


# Kalman Filter
for n in range(len(measurements[0])):
    
    # Time Update (Prediction)
    # ========================
    # Project the state ahead
    x = A*x
    
    # Project the error covariance ahead
    P = A*P*A.T + Q

    
    # Measurement Update (Correction)
    # ===============================
    # Compute the Kalman Gain
    S = H*P*H.T + R
    K = (P*H.T) * np.linalg.pinv(S)

    # Update the estimate via z
    Z = measurements[:,n].reshape(2,1)
    y = Z - (H*x)                            # Innovation or Residual
    x = x + (K*y)
    
    # Update the error covariance
    P = (I - (K*H))*P

    
    # Save states for Plotting
    xt.append(float(x[0]))
    yt.append(float(x[1]))


# State Estimate: Position (x,y)
fig = plt.figure(figsize=(16,16))
plt.scatter(xt,yt, s=20, label='State', c='k')
plt.scatter(xt[0],yt[0], s=100, label='Start', c='g')
plt.scatter(xt[-1],yt[-1], s=100, label='Goal', c='r')

plt.xlabel('X')
plt.ylabel('Y')
plt.title('Position')
plt.legend(loc='best')
plt.axis('equal')
plt.show()

汽车在隧道中的估计位置如下图：

 

 参考

Improving IMU attitude estimates with velocity data

https://zhuanlan.zhihu.com/p/25598462