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

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

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

KF的应用

目标的运动模型

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

$$x_t=x_{t-1}+v_{t-1}∆t+\frac{1}{2}a∆t^2(1)$$

$$v_t=v_{t-1}+a\Delta t(2)$$

这里我们将最后一项 $\frac{1}{2}a∆t^2$，$a\Delta t$考虑为系统模型不准确带来的噪声(注意$a$是一个随机变量，$\Delta t$是一个确定的时间间隔)，$a^2\sim N(0,Q)$，我们可以将上面的公式合并在一起：
$$X_t=\left[\begin{array}{c}{x}_t\\ v_t\end{array}\right]=\left[\begin{array}{cc}1& ∆t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{t-1}\\ v_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{1}{2}a∆t^2\\ a\Delta t\end{array}\right](3)$$

所以t时刻状态的均值为：
$$E[X_t]=\left[\begin{array}{cc}1& ∆t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{t-1}\\ v_{t-1}\end{array}\right]=A{X}_{t-1}(4)$$

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

$$V[X_t]=E[(X_t-E[X_t])(X_t-E[X_t]{)}^T](5)$$

$$\begin{gathered} V[X_t]=E\left[\left[\begin{array}{c}\frac{1}{2}a∆t^2\\ a∆t\end{array}\right]\left[\begin{array}{cc}\frac{1}{2}a∆t^2& a∆t\end{array}\right]\right]=E\left[\begin{array}{cc}\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{array}\right] \\ =E[a^2]\left[\begin{array}{cc}\frac{1}{4}∆t^4& \frac{1}{2}∆t^3\\ \frac{1}{2}∆t^3& ∆t^2\end{array}\right]=Q\left[\begin{array}{cc}\frac{1}{4}∆t^4& \frac{1}{2}∆t^3\\ \frac{1}{2}∆t^3& ∆t^2\end{array}\right](6) \end{gathered}$$

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

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

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

$${\bar{\Sigma }}_t=A{\hat{\Sigma }}_{t-1}{A}^T+Q(8)$$

$$K_t={\bar{\Sigma }}_t{H}^T(H{\bar{\Sigma }}_t{H}^T+R{)}^{-1}(9)$$

$${\hat{\mu }}_t={\bar{\mu }}_t+K_t(Y_t-H{\bar{\mu }}_t)(10)$$

$${\hat{\Sigma }}_t=(I-K_{t}H){\bar{\Sigma }}_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$。

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

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

KF的调试

1.状态转移方程（${\bar{\mu }}_t$$=A$${\hat{\mu }}_{t-1}$）的建立
对一个系统的过程建模的越准确，意味着通过状态转移得到的状态越可靠，这样过程噪声Q要相对更小些。相反如果对系统的过程不太清楚，那么选择的状态转移模型就会不准确，所以要更相信测量值，要把过程噪声Q相对调大些。如果状态转移模型很不准确但是过程噪声设的非常小，那么卡尔曼滤波算法性能将严重下降甚至发散。
2. 系统的过程噪声 Q
系统的过程噪声 Q 越大，预测的状态协方差 ${\bar{\Sigma }}_t$（$=A$${\hat{\Sigma }}_{t-1}{A}^T$+Q ）就越大，卡尔曼增益 $K_t$ 越大，观测的权重越大，预测的状态的权重越小，表示越相信观测，越不相信状态转移过程，这时卡尔曼滤波算法对量测的变化响应快，但是越不容易收敛。

$$K_t=\frac{{\bar{\Sigma }}_t{H}^T}{(H{\bar{\Sigma }}_t{H}^T+R)}$$

$${\hat{\mu }}_t={\bar{\mu }}_t+K_t(Y_t-H{\bar{\mu }}_t)=(I-K_{t}H){\bar{\mu }}_t+K_t{Y}_t$$

反之，系统的过程噪声 Q 越小，表示越相信预测的状态，越不相信观测，这是系统状态越容易收敛，对观测的变化响应越慢。
3.量测噪声 R
首先，传感器的量测越不准确当然量测噪声 R 越大，卡尔曼增益 $K_t$ 越小，预测的状态的权重越大，量测的权重越小，表示越相信预测的状态，越不相信量测，这是系统状态越容易收敛，对观测的变化响应越慢。

参考资料

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