滤波系列（一）卡尔曼滤波算法（KF）：应用

滤波系列（一）卡尔曼滤波算法的应用

在本文，将直接给出卡尔曼滤波算法的具体应用，如果想了解卡尔曼滤波算法的详细数学推导过程，请看博客：卡尔曼滤波算法的详细数学推导

KF的应用

目标的运动模型

举一个物体做匀速直线运动的例子：
物体的状态由位置和速度组成定义为 $X_t=[x_t,v_t ]^T$Xt​=[xt​,vt​]T，根据匀速直线运动公式可知：

$x_t=x_{t-1}+v_t ∆t+\frac{1}{2} a∆t^2 \quad(1)$xt​=xt−1​+vt​∆t+21​a∆t2(1)

$v_t=v_{t-1}+aΔt \quad(2)$vt​=vt−1​+aΔt(2)

这里我们将最后一项 $\frac{1}{2} a∆t^2$21​a∆t2，$aΔt$aΔt考虑为系统模型不准确带来的噪声(注意$a$a是一个随机变量，$Δt$Δt是一个确定的时间间隔)，$a^2\sim N(0,Q)$a2∼N(0,Q)，我们可以将上面的公式合并在一起：
$X_t= \begin{bmatrix} x_t\\ v_t \\ \end{bmatrix}= \begin{bmatrix} 1&∆t\\ 0&1 \\ \end{bmatrix} \begin{bmatrix} x_{t-1}\\ v_{t-1} \\ \end{bmatrix}+ \begin{bmatrix} \frac{1}{2} a∆t^2\\ aΔt \\ \end{bmatrix} \quad (3)$Xt​=[xt​vt​​]=[10​∆t1​][xt−1​vt−1​​]+[21​a∆t2aΔt​](3)

所以t时刻状态的均值为：
$E[X_t ]=\begin{bmatrix} 1&∆t\\ 0&1 \\ \end{bmatrix} \begin{bmatrix} x_{t-1}\\ v_{t-1} \\ \end{bmatrix}=AX_{t-1}\quad (4)$E[Xt​]=[10​∆t1​][xt−1​vt−1​​]=AXt−1​(4)

所以t时刻状态的方差为：
注：随机变量求方差公式 $V[X]=E[(X-μ) (X-μ)^T]$V[X]=E[(X−μ)(X−μ)T]

$V[X_t ]=E[(X_t-E[X_t ]) (X_t-E[X_t ])^T] (5)$V[Xt​]=E[(Xt​−E[Xt​])(Xt​−E[Xt​])T](5)

$V[X_t ]=E\left[ \begin{bmatrix} \frac{1}{2}a∆t^2\\ a∆t\\ \end{bmatrix} \begin{bmatrix} \frac{1}{2}a∆t^2&a∆t\\\end{bmatrix}\right] =E \begin{bmatrix} \frac{1}{4} a^2 ∆t^4&\frac{1}{2} a^2 ∆t^3\\ \frac{1}{2} a^2 ∆t^3&a^2 ∆t^2\\\end{bmatrix} \\=E[a^2 ] \begin{bmatrix} \frac{1}{4} ∆t^4&1/2 ∆t^3\\ 1/2 ∆t^3&∆t^2\\\end{bmatrix} =Q\begin{bmatrix} \frac{1}{4} ∆t^4&1/2 ∆t^3\\ 1/2 ∆t^3&∆t^2\\\end{bmatrix}\quad (6)$V[Xt​]=E[[21​a∆t2a∆t​][21​a∆t2​a∆t​]]=E[41​a2∆t421​a2∆t3​21​a2∆t3a2∆t2​]=E[a2][41​∆t41/2∆t3​1/2∆t3∆t2​]=Q[41​∆t41/2∆t3​1/2∆t3∆t2​](6)

传感器的量测为 $Y_t=[x_t ]$Yt​=[xt​]，量测方程为 $H=\begin{bmatrix}1&0\end{bmatrix}$H=[1​0​]。
假设目标的初始时刻的状态为 $X_1=[10,5]^T$X1​=[10,5]T、协方差矩阵为 $P_0=\begin{bmatrix}10&5\\5&10\\\end{bmatrix}$P0​=[105​510​]，采样时间间隔 $∆t=0.1s$∆t=0.1s，传感器的量测噪声为 $R=[1]$R=[1]。由此产生的仿真数据如下图：

图1：目标的真实位置及量测

其中，蓝色的线表示加了噪声的量测数据，橘色的线表示真实的位置。
卡尔曼滤波的过程：
$\bar{\mu}_t=A\hat{\mu}_{t-1} \quad(7)$μˉ​t​=Aμ^​t−1​(7)

$\bar{\Sigma}_t=A\hat{\Sigma}_{t-1} A^T+Q \quad(8)$Σˉt​=AΣ^t−1​AT+Q(8)

更新公式：

$K_t=\bar{\Sigma}_t H^T (H\bar{\Sigma}_t H^T+R)^{-1}\quad (9)$Kt​=Σˉt​HT(HΣˉt​HT+R)−1(9)

$\hat{\mu}_t=\bar{\mu}_t+K_t (Y_t-H\bar{\mu}_t) \quad (10)$μ^​t​=μˉ​t​+Kt​(Yt​−Hμˉ​t​)(10)

$\hat{\Sigma}_t =(I-K_t H) \bar{\Sigma}_t \quad (11)$Σ^t​=(I−Kt​H)Σˉt​(11)

卡尔曼滤波代码

def kalmanfilter(A, H, Q, R, z, P, x):
    
    n = A.shape[1]
    #Predict
    x = np.dot(A, x)  #公式(7)
    P = multi_dot([A, P, A.T]) + Q #公式(8)
    #Update
    y = z - np.dot(H, x) 
    S = np.matrix(multi_dot([H, P, H.T])+R) 
    K = multi_dot([P, H.T, inv(S)]) #公式(9)
    x = x + np.dot(K, y) #公式(10)
    I = np.eye(n)
    P = np.dot(I - np.dot(K, H), P) #公式(11)
    
    return x,P

完整代码

import numpy as np
from numpy.linalg import multi_dot,inv
import matplotlib.pyplot as plt
np.random.seed(2)

def kalmanfilter(A, H, Q, R, z, P, x):
    
    n = A.shape[1]
    #Predict
    x = np.dot(A, x) 
    P = multi_dot([A, P, A.T]) + Q
    #Update
    y = z - np.dot(H, x)
    S = np.matrix(R + multi_dot([H, P, H.T]))
    K = multi_dot([P, H.T, inv(S)])
    x = x + np.dot(K, y)
    I = np.eye(n)
    P = np.dot(I - np.dot(K, H), P)
    
    return x,P
                 	
def demo():
    
    dt = 0.1
    F = np.array([[1, dt], [0, 1]])
    H = np.array([1, 0]).reshape(1,2)
    Q = 1e-2*np.array([[1/4*dt**4, 1/2*dt**3], [1/2*dt**3, dt**2]])
    R = 2
    itr  = 100
    
    real_state = []
    x = np.array([10,5]).reshape(2,1)
    
    for i in range(itr):
        real_state.append(x[0,0])
        x = np.dot(F,x)+np.random.multivariate_normal(mean=(0,0),cov=Q).reshape(2,1)
    
    measurements = [x+np.random.normal(0,R) for x in real_state]
    
    # initialization
    P = np.array([[10,5],[5,10]])
    x=np.random.multivariate_normal(mean=(10,5),cov=P).reshape(2,1)
    
    #filter
    filter_result=list()
    filter_result.append(x)
    for i in range(1,itr):
        x,P = kalmanfilter(F, H, Q, R, measurements[i],P,x)
        filter_result.append(x)
    filter_result=np.squeeze(np.array(filter_result))
    
    return measurements,real_state,filter_result

def plot_result(measurements,real_state,filter_result):
    
    plt.figure(figsize=(8,4))
    plt.plot(range(1,len(measurements)), measurements[1:], label = 'Measurements')
    plt.plot(range(1,len(real_state)), real_state[1:], label = 'Real statement' )
    plt.plot(range(1,len(filter_result)), np.array(filter_result)[1:,0], label = 'Kalman Filter')
    plt.legend()
    plt.xlabel('Time',fontsize=14)
    plt.ylabel('velocity [m]',fontsize=14)
    plt.show()
    
    plt.figure(figsize=(8,4))
    plt.axhline(5, label='Real statement') #, label='$GT_x(real)$'
    plt.plot(range(1,len(filter_result)), np.array(filter_result)[1:,1], label = 'Kalman Filter')
    plt.legend()
    plt.xlabel('Time',fontsize=14)
    plt.ylabel('velocity [m]',fontsize=14)
    plt.show()

if __name__ == '__main__':
    
    measurements,real_state,filter_result=demo()
    plot_result(measurements,real_state,filter_result)

滤波后的结果如下图所示，图2表示目标位置的滤波结果，从图中可直观发现经过卡尔曼滤波后，消除了量测数据中的噪声，得到了更精确的目标位置；图3表示目标速度的滤波结果，从中可以发现经过卡尔曼滤波后，速度的估计越来越精确且逐渐收敛到了真实的速度$v=5m/s$v=5m/s。

图2：目标位置的滤波结果

图3：目标速度的滤波结果

参考资料

卡尔曼滤波算法的详细数学推导