文章目錄
- 關鍵代碼
- 解釋
- 例子
關鍵代碼
import numpy as np
#一步預測
def kf_predict(X0, P0, A, Q, B, U1):
X10 = np.dot(A,X0) + np.dot(B,U1)
P10 = np.dot(np.dot(A,P0),A.T)+ Q
return (X10, P10)
#測量更新
def kf_update(X10, P10, Z, H, R):
K = np.dot(np.dot(P10,H.T),np.linalg.pinv(np.dot(np.dot(H,P10),H.T) + R))
X1 = X10 + np.dot(K,Z - np.dot(H,X10))
P1 = np.dot(1 - np.dot(K,H),P10)
return (X1, P1, K)
解釋
離散的狀態方程、觀測方程及它們的隨機程序如下：
X
(
k
)
=
A
X
(
k
?
1
)
+
B
U
(
k
)
+
w
(
k
?
1
)
Z
(
k
)
=
H
X
(
k
)
+
e
(
k
)
p
(
w
)
=
N
(
0
,
Q
)
p
(
e
)
=
N
(
0
,
R
)
X(k) = AX(k-1) + BU(k) + w(k-1)\\ Z(k) = HX(k) + e(k)\\ p(w) = N(0, Q)\\ p(e) = N(0, R)\\
X(k)=AX(k?1)+BU(k)+w(k?1)Z(k)=HX(k)+e(k)p(w)=N(0,Q)p(e)=N(0,R)
如果是連續的狀態方程則需要離散化，例如上式中的A等于： A = e x p m ( F Δ t ) A = expm(F\Delta t) A=expm(FΔt)
其中expm指矩陣指數，F為微分運動方程 X ˙ = F X \dot X=FX X˙=FX線性化后的系數矩陣，可以使用sympy.exp協助推導，（線性化及離散化不屬于本文范圍）
Kalman Filter主要步驟為一步預測和測量更新兩個部分，以下列出Kalman黃金5公式
一步預測
X
(
k
,
k
?
1
)
=
A
X
(
k
?
1
)
+
B
U
(
k
)
P
(
k
,
k
?
1
)
=
A
P
(
k
?
1
)
A
T
+
Q
X(k,k-1)=AX(k-1)+BU(k)\\ P(k,k-1)=AP(k-1)A^T+Q
X(k,k?1)=AX(k?1)+BU(k)P(k,k?1)=AP(k?1)AT+Q
測量更新：
K
(
k
)
=
P
(
k
,
k
?
1
)
H
T
[
H
P
(
k
,
k
?
1
)
H
T
+
R
]
?
1
X
(
k
)
=
X
(
k
,
k
?
1
)
+
K
(
k
)
[
Z
(
k
)
?
H
X
(
k
,
k
?
1
)
]
P
(
k
)
=
[
I
?
K
(
k
)
]
P
(
k
,
k
?
1
)
K(k)=P(k,k-1)H^T[HP(k,k-1)^HT+R]^{-1}\\ X(k)=X(k,k-1)+K(k)[Z(k)-HX(k,k-1)] P(k)=[I-K(k)]P(k,k-1)
K(k)=P(k,k?1)HT[HP(k,k?1)HT+R]?1X(k)=X(k,k?1)+K(k)[Z(k)?HX(k,k?1)]P(k)=[I?K(k)]P(k,k?1)
import numpy as np
#一步預測
'''
設狀態量有xn個
- X0為前一時刻狀態量，shape=(xn,1)
- P0為初始狀態不確定度， shape=(xn,xn)
- A為狀態轉移矩陣，shape=(xn,xn)
- Q為遞推噪聲協方差矩陣，shape=(xn,xn)
- B、U1是外部輸入部分
回傳的結果為
- X10為一步預測的狀態量結果，shape=(xn,1)
- P10為一步預測的協方差，shape=(xn,xn)
'''
def kf_predict(X0, P0, A, Q, B, U1):
X10 = np.dot(A,X0) + np.dot(B,U1)
P10 = np.dot(np.dot(A,P0),A.T)+ Q
return (X10, P10)
'''
設狀態量有xn個
- X10為一步預測的狀態量結果，shape=(xn,1)
- P10為一步預測的協方差，shape=(xn,xn)
- Z為觀測值，shape=(xn,1)
- H為觀測系數矩陣，shape=(xn,xn)
- R為觀測噪聲協方差，shape=(xn,xn)
回傳的結果為
- X1為一步預測的狀態量結果，shape=(xn,1)
- P1為一步預測的協方差，shape=(xn,xn)
- K為卡爾曼增益，不需要回傳，但是可以看一下它的值來判斷是否正常運行
'''
#測量更新
def kf_update(X10, P10, Z, H, R):
K = np.dot(np.dot(P10,H.T),np.linalg.pinv(np.dot(np.dot(H,P10),H.T) + R))
X1 = X10 + np.dot(K,Z - np.dot(H,X10))
P1 = np.dot(1 - np.dot(K,H),P10)
return (X1, P1, K)
注意在Numpy shape(n,) 不等于shape(n,1)
例子
以勻加速度運動為例，結果如下，代碼在最后
可見前期偏預測、后期偏觀測
# -*- coding: utf-8 -*-
"""
Created on Wed Mar 31 16:02:39 2021
@author: Canvas
@function: Kalman Filter Demo
"""
import numpy as np
import matplotlib.pyplot as plt
"""
X(k) = AX(k-1) + BU(k) + w(k-1)
Z(k) = HX(k) + e(k)
p(w) = N(0, Q)
p(e) = N(0, R)
"""
def kf_predict(X0, P0, A, Q, B, U1):
X10 = np.dot(A,X0) + np.dot(B,U1)
P10 = np.dot(np.dot(A,P0),A.T)+ Q
return (X10, P10)
def kf_update(X10, P10, Z, H, R):
V = Z - np.dot(H,X10)
K = np.dot(np.dot(P10,H.T),np.linalg.pinv(np.dot(np.dot(H,P10),H.T) + R))
X1 = X10 + np.dot(K,V)
P1 = np.dot(1 - np.dot(K,H),P10)
return (X1, P1, K)
"""
加速度白噪聲建模
狀態方程：
x' = v'
v' = a'
a' = 0
離散化得到；
x(k) = x(k-1)+t*v(k)+0.5*t^2*a(k)
v(k) = v(k-1)+t*a(k)
a(k) = a(k-1)
觀測方程：
z(k) = x(k) + e
"""
n = 101 #資料量
nx = 3 #變數數量
t = np.linspace(0,5,n) #時間序列
dt = t[1] - t[0]
#真實函式關系
a_true = np.ones(n)*9.8
v_true = a_true*t
x_true = 0.5*a_true*(t**2)
X_true = np.concatenate([x_true, v_true, a_true]).reshape([nx,-1])
# 觀測噪聲協方差！！！！！！！！！！！！！！！！！！！！（可調整）
R = np.diag([5**2,0,0])
#仿真觀測值
e = np.random.normal(0,np.sqrt(R[0][0]),n)
x_obs = X_true[0,:]
x_obs += e
Z = np.zeros([nx,n])
Z[0,:] = x_obs
# 計算系數
A = np.array([1,dt,0.5*dt**2,
0,1,dt,
0,0,1]).reshape([nx,nx])
B = 0
U1 = 0
#狀態假設（觀測）初始值
x0 = -1.0
v0 = 1.0
a0 = 9.0
X0 = np.array([x0,v0,a0]).reshape(-1,1)
#初始狀態不確定度！！！！！！！！！！！！！！！！（可調整）
P0 = np.diag([5**2,2**2,1**2])
#狀態遞推噪聲協方差！！！！！！！！！！！！！！！！！！（可調整）
Q = np.diag([0,0,1.0**2])
###開始處理
X1_np = np.copy(X0)
P1_list = [P0]
X10_np = np.copy(X0)
P10_list = [P0]
for i in range(n):
Zi = np.array(Z[:,i]).reshape([-1,1])
Hi = np.array([1,0,0,
0,0,0,
0,0,0]).reshape([nx,nx])
if (i == 0):
continue
else:
Xi = X10_np[:,i-1].reshape([-1,1])
Pi = P10_list[i-1]
X10, P10 = kf_predict(Xi, Pi, A, Q, B, U1)
X10_np = np.concatenate([X10_np, X10], axis=1)
P10_list.append(P10)
X1, P1, K = kf_update(X10, P10, Zi, Hi, R)
X1_np = np.concatenate([X1_np, X1], axis=1)
P1_list.append(P1)
#結束，繪圖
fig = plt.figure()
ax1 = fig.add_subplot(1,1,1)
ax1.plot(x_true, 'k-', label="Truth")
ax1.plot(X1_np[0,:], 'go--', label="Kalman Filter")
ax1.plot(X10_np[0,:], 'ro--', label="Prediction")
ax1.scatter(np.arange(n), Z[0,:], label="Observation", marker='*')
plt.legend()
plt.show()
轉載請註明出處，本文鏈接：https://www.uj5u.com/houduan/274065.html
標籤：python