案例場景
傳感器能夠直接觀測到某行人的速度Vx,Vy，用卡爾曼濾波估算該行人的狀态(包含速度和位置）
相關變量約定如下：
行人狀态
P: 行人不确定性，協方差矩陣
F: 狀态轉移矩陣
Q: 過程噪聲協方差矩陣
K: 卡爾曼濾波增益
H: 觀測矩陣
R: 觀測噪聲協方差矩陣
step0:随機産生一批測量資料，包含二維速度測量值
import numpy as np
%matplotlib inline
import matplotlib.pyplot as plt
from scipy.stats import norm
m = 200 #measurements
vx = 20
vy = 10
mx = np.array(vx + np.random.randn(m))
my = np.array(vy + np.random.randn(m))
measurements = np.vstack((mx,my))
print(measurements.shape)
print('Standard Deviation of Acceleration Measurements=%0.2f'%np.std(mx))
print('You assumed %0.2f in R.'%R[0,0]) 執行結果：
(2, 200)
Standard Deviation of Acceleration Measurements=1.04
You assumed 0.09 in R.
fig = plt.figure(figsize=(16,5))
plt.step(range(m), mx, label='$\dot x $')
plt.step(range(m), my, label='$\dot y $')
plt.ylabel(r'Velocity $m/s$')
plt.title('Measurements')
plt.legend(loc='best',prop={'size':18})
plt.savefig('measurements.png') 
step1:初始化行人狀态
包括x,y方向的位置和速度，及行人的不确定性，測量間隔時間dt
x: 行人狀态初始值, 因未知，是以全設定為0
P: 行人不确定性初始值，不确定性很高
x = np.matrix([[0.0,0.0,0.0,0.0]]).T
print(x,x.shape)
P=np.diag([1000.0,1000.0,1000.0,1000.0])
print(P,P.shape) 執行結果：
[[0.]
[0.]
[0.]
[0.]] (4, 1)
[[1000. 0. 0. 0.]
[ 0. 1000. 0. 0.]
[ 0. 0. 1000. 0.]
[ 0. 0. 0. 1000.]] (4, 4)
step2:設計過程模型和過程噪聲協方差矩陣
設計卡爾曼濾波器時，必須定義兩個線性函數，如下圖：
狀态轉移函數F：該函數對從時間k-1到時間k的狀态變換進行模組化;
測量函數H： 該函數對測量值的計算方式，以及測量值和預測值狀态x的關聯進行模組化;
這些函數的第一部分F，H是模型中的确定性部分，尾項噪聲v和噪聲w表述的時随機部分，影響預測和測量更新步驟的随機誤差；
(1)假設運動為恒速模型，即行人速度不變，過程模型可以描述如下
即運動模型為：
(2)實際上行人運動過程不一定為恒速，總會有内因(行人自己加速度，無人車内部的加速度控制)和外因(風速，路面光滑程度)等影響；
内因用u表示，是行人或無人車内部控制向量，B是輸入控制矩陣；
Bu表示行人由于自身内部動力，引起狀态變化；
v表示由于風速，路滑等外因引起的狀态變化量，是個随機變量，稱為過程噪聲；
對一個行人連續觀察兩次，獲得初始速度和最終速度，根據根據動力學公式，可以推導出目前時刻狀态和上一時刻狀态的函數關系，
由于觀測物體(行人)自身加速度無法準确和預估，應用中常設定Bu=0, 就用随機變量v(均值為0的高斯噪聲)作為随機噪聲；
由于加速度未知，可以把加速度加到誤差分量中，添加v噪聲（加速度和過程噪聲都用v來表征）後的過程模型為：
矩陣表示為：
将v分解為兩個分量：一個4x2的矩陣G(不包含随機變量)和一個2x1的矩陣a(包含随機加速度分量)。
根據誤差向量，現在可以定義新的協方差矩陣Q：協方差矩陣定位為誤差向量的期望;
因為G不包含随機變量，可以放在期望外面
合并回原矩陣得到如下過程協方差矩陣
噪聲Q本質上是一個均值為0的高斯分布v~N(0,Q), 卡爾曼公式2就變成：
轉移矩陣表示為：
dt = 0.1 # Time step between Filters steps
F = 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]])
print(F,F.shape) 執行結果：
[[1. 0. 0.1 0. ]
[0. 1. 0. 0.1]
[0. 0. 1. 0. ]
[0. 0. 0. 1. ]] (4, 4)
‘’‘
sv = 0.5
G = np.matrix([[0.5*dt**2],
[0.5*dt**2],
[dt],
[dt]])
Q = G*G.T*sv*2
from sympy import Symbol, Matrix
from sympy.interactive import printing
printing.init_printing()
dts = Symbol('dt')
’‘’
noise_ax=0.5
noise_ay=0.5
dt_2 = dt*dt;
dt_3 = dt_2 *dt;
dt_4 = dt_3*dt;
Q = np.matrix([[0.25*dt_4*noise_ax,0,0.5*dt_3*noise_ax,0],
[0, 0.25*dt_4*noise_ay,0, 0.25*dt_3*noise_ay],
[dt_3/2*noise_ax, 0, dt_2*noise_ax, 0],
[0, dt_3/2*noise_ay, 0, dt_2*noise_ay]]) 執行結果：
[[0.09 0. ]
[0. 0.09]] (2, 2)
step3: 設計測量模型，觀測噪聲
(1)使用傳感器可以直接測量行人的速度Vx, Vy，
(2) 測量矩陣可以表示為：
(3)測量噪聲的協方差矩陣為：
描述了傳感器的測量有“多差”，是傳感器固有性質，一般有廠商提供
H = np.matrix([[0.0,0.0,1.0,0.0],
[0.0,0.0,0.0,1.0]])
print(H, H.shape)
ra = 0.09 #廠商提供
R = np.matrix([[ra,0.0],
[0.0,ra]])
print(R,R.shape) 執行結果：
[[0. 0. 1. 0.]
[0. 0. 0. 1.]] (2, 4)
[[0.09 0. ]
[0. 0.09]] (2, 2)
I = np.eye(4)
print(I, I.shape) 執行結果：
[[1. 0. 0. 0.]
[0. 1. 0. 0.]
[0. 0. 1. 0.]
[0. 0. 0. 1.]] (4, 4)
一些過程值，用于結果顯示
xt = []
yt = []
dxt = []
dyt = []
Zx = []
Zy = []
Px = []
Py = []
Pdx = []
Pdy = []
Rdx = []
Rdy = []
Kx = []
Ky = []
Kdx =[]
Kdy = []
def save_states(x,Z,P,R,K):
xt.append(float(x[0]))
yt.append(float(x[1]))
dxt.append(float(x[2]))
dyt.append(float(x[3]))
Zx.append(float(Z[0]))
Zy.append(float(Z[1]))
Px.append(float(P[0,0]))
Py.append(float(P[1,1]))
Pdx.append(float(P[2,2]))
Pdy.append(float(P[3,3]))
Rdx.append(float(R[0,0]))
Rdy.append(float(R[1,1]))
Kx.append(float(K[0,0]))
Ky.append(float(K[1,0]))
Kdx.append(float(K[2,0]))
Kdy.append(float(K[3,0])) step4: 卡爾曼公式
for n in range(len(measurements[0])):
#Time Update(Prediction)
# ==============================
x = F*x #Project the state ahead
P = F * P *F.T + Q #Project the error covariance ahead
# Measurement Update (Correction)
#==============================
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)
x = x + (K*y)
#update the error convariance
P = (I - (K*H))*P
#save states (for Plotting)
save_states(x,Z,P,R,K) def plot_x():
fig = plt.figure(figsize=(16,9))
plt.step(range(len(measurements[0])), dxt, label='$estimateVx $')
plt.step(range(len(measurements[0])), dyt, label='$estimateVy $')
plt.step(range(len(measurements[0])),measurements[0],label='$measurementVx$')
plt.step(range(len(measurements[0])),measurements[1],label='$measurementVy$')
#plt.axhline(vx, colors='#999999',label = '$trueVx$')
#plt.axhline(vy, colors='#999999',label = '$trueVy$')
plt.xlabel('Filter Step')
plt.title('Estimate (Elements from State Vector $x$)')
plt.legend(loc='best',prop={'size':11})
plt.ylim([0,30])
plt.ylabel('Velocity')
plot_x() 卡爾曼濾波關于速度的估計結果
def plot_xy():
fig = plt.figure(figsize=(16,9))
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.legend('Position')
plt.axis('equal')
plot_xy()