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

## KF的应用

### 目标的运动模型

x t = x t − 1 + v t − 1 ∆ t + 1 2 a ∆ t 2 ( 1 ) x_t=x_{t-1}+v_{t-1} ∆t+\frac{1}{2} a∆t^2 \quad(1)

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

X t = [ x t v t ] = [ 1 ∆ t 0 1 ] [ x t − 1 v t − 1 ] + [ 1 2 a ∆ t 2 a Δ t ] ( 3 ) 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)

E [ X t ] = [ 1 ∆ t 0 1 ] [ x t − 1 v t − 1 ] = A X t − 1 ( 4 ) 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)

V [ X t ] = E [ ( X t − E [ X t ] ) ( X t − E [ X t ] ) T ] ( 5 ) V[X_t ]=E[(X_t-E[X_t ]) (X_t-E[X_t ])^T] (5)

V [ X t ] = E [ [ 1 2 a ∆ t 2 a ∆ t ] [ 1 2 a ∆ t 2 a ∆ t ] ] = E [ 1 4 a 2 ∆ t 4 1 2 a 2 ∆ t 3 1 2 a 2 ∆ t 3 a 2 ∆ t 2 ] = E [ a 2 ] [ 1 4 ∆ t 4 1 2 ∆ t 3 1 2 ∆ t 3 ∆ t 2 ] = Q [ 1 4 ∆ t 4 1 2 ∆ t 3 1 2 ∆ t 3 ∆ t 2 ] ( 6 ) 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&\frac{1}{2} ∆t^3\\ \frac{1}{2} ∆t^3&∆t^2\\\end{bmatrix} =Q\begin{bmatrix} \frac{1}{4} ∆t^4&\frac{1}{2} ∆t^3\\ \frac{1}{2} ∆t^3&∆t^2\\\end{bmatrix}\quad (6)

μ ˉ t = A μ ^ t − 1 ( 7 ) \bar{\mu}_t=A\hat{\mu}_{t-1} \quad(7)

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

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

μ ^ t = μ ˉ t + K t ( Y t − H μ ˉ t ) ( 10 ) \hat{\mu}_t=\bar{\mu}_t+K_t (Y_t-H\bar{\mu}_t) \quad (10)

Σ ^ t = ( I − K t H ) Σ ˉ t ( 11 ) \hat{\Sigma}_t =(I-K_t H) \bar{\Sigma}_t \quad (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)


## KF的调试

1.状态转移方程（ μ ˉ t \bar{\mu}_t = A =A μ ^ t − 1 \hat{\mu}_{t-1} ）的建立

2. 系统的过程噪声 Q

K t = Σ ˉ t H T ( H Σ ˉ t H T + R ) K_t=\frac{\bar{\Sigma}_t H^T }{(H\bar{\Sigma}_t H^T+R)}

μ ^ t = μ ˉ t + K t ( Y t − H μ ˉ t ) = ( I − K t H ) μ ˉ t + K t Y t \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

3.量测噪声 R

11-06

05-04 3万+
12-22 941
04-09 2453
12-04 2152
09-10 1万+
05-16 1226
02-11 8万+
04-22 672
08-19 462
05-06 5500