卡尔曼滤波 （Kalman Filter）的一个简单实现： 恒定加速度模型

恒定加速度Kalman模型

1. scenario：我们想用卡尔曼滤波来追踪3D空间中某个具有恒定加速度的物体，我们有一个位置传感器可以用来观测物体位置，我们想得到物体的3D位置以及速度。

2. Description :

假设物体状态由六维vector定义如下：

 

 

系统的预测方程为：

 

 

假设我们没有控制输入，那么预测方程可以表示如下：

y = Hx

因为我们有位置传感器，不管是视觉，激光还是雷达，我们可以得到物体在3D空间中的位置，即（x，y，z），那么系统的观测方程可以用如下表示：

接下来我们定义初始状态的协方差矩阵为9x9的单位矩：

 

接下来定义系统的状态转移矩阵A，若定义状态间的time step为 dt = 1，则此时的A矩阵为：

 

定义系统的初始观测协方差矩阵为3x3的对角阵：

:系统的加速度扰动 ,那么Q可以用如下表示：

 

3. Implementation ：

 

这里省略了一些细节，具体代码请查看我的github主页：https://github.com/Doodleshr

 

 

P = 100.0*np.eye(9)

#define dt = 0.05 rather than dt = 1
dt = 0.05

#define H and A according to what we have discused
......
###
#define observation noise covariance
R = 100*np.eye(3)

#define process noise matrix Q
#from the blog we mentioned how to compute Q from G
G = ...
a = 0.1
Q = G*G.T*a**2


#system initialization
px=0.0
py=0.0
pz=0.0

vx=1
vy=2
vz=3

Xr=[]
Yr=[]
Zr=[]

# #generate position:
for i in range(100):

    # we assume constant acceleratoin for this demo
    accx = 3
    vx += accx * dt
    px += vx * dt

    accy = 1
    vy += accy * dt
    py += vy * dt

    accz = 1
    vz += accz * dt
    pz += vz * dt

    Xr.append(px)
    Yr.append(py)
    Zr.append(pz)

    # position noise
    sp = 0.5

Xm = Xr + sp * (np.random.randn(100))
Ym = Yr + sp * (np.random.randn(100))
Zm = Zr + sp * (np.random.randn(100))

#stack measurements
measurements = np.vstack((Xm,Ym,Zm))

#define initial state
X = np.matrix([0.0, 0.0, 0.0, 1.0, 2.0, 3.0, 3.0, 1.0, 1.0]).T
I = np.eye(9)
Xx = []
Xy = []
Xz = []

for each_step in range(100):

    #prediction step:
    X = A*X
    #state covariance propogation
    P = A*P*A.T + Q

    #measurement covariance propogation
    S = H*P*H.T + R

    #Kalman Gain update
    K = (P*H.T) * np.linalg.pinv(S)
    # print(K)
    Z = measurements[:,each_step].reshape(3,1)

    #corrected/updated x
    X = X + K*(Z-(H*X))

    #update state covariance P:
    P = (I - (K * H)) * P

    #store corrected/updated x
    Xx.append(float(X[0]))
    Xy.append(float(X[1]))
    Xz.append(float(X[2]))

 

 

4. Result:

 

我们可以看到观测值是很noisy的，但是使用卡尔曼滤波后，轨迹变得平滑许多，也更加贴合实际轨迹，可见卡尔曼滤波是是有效的。