[英]Error state Kalman Filter from MATLAB to Python
我試圖重現這里用Python解釋的算法,但我遇到了一些參數奇怪增長的問題。 以下是我的嘗試。 觀察到get_ang()
和get_acc()
沿[x,y,z]軸返回角速度和加速度(以度/秒為單位 (但我將這些數據以弧度/秒轉換)和m / s ^ 2 ):
import numpy as np
import quaternion
from utils import get_ang, get_acc
#utils
Z=np.zeros([3,3])
I=np.eye(3)
EARTH_GRAVITY_MS2 = -9.80665
#sample parameters
N=1 #DecimationFactor
fs=10 #SampleRate
#noise parameters
beta=3.0462e-13 #GyroscopeDriftNoise
eta=9.1385e-5 #GyroscopeNoise
kappa=N/fs #DecimationFactor/SampleRate
lamb=0.00019247 #AccelerometerNoise
nu=0.5 #LinearAccelerationDecayFactor
csi=0.0096236 #LinearAccelerationNoise
#other parameters initialization
lin_acc_prior=np.zeros(3)
gyro_offset=np.zeros([1,3])
Q=np.diag([0.000006092348396, 0.000006092348396, 0.000006092348396, 0.000076154354947,0.000076154354947, 0.000076154354947,0.009623610000000, 0.009623610000000, 0.009623610000000])
R=(lamb+csi+(kappa**2)*(beta+eta))*I
P=Q
q=quaternion.quaternion(1,0,0,0)
while(1):
"""----------------------------------------------------------Model----------------------------------------------------------"""
"""Predict orientation (q-)"""
gyro_readings=np.array(np.radians([get_ang()])) #rad/s
for i in range(N-1):
gyro_readings=np.append(gyro_readings,np.radians([get_ang()]),axis=0)
delta_phi=(gyro_readings-gyro_offset)/fs #rad/s
delta_q=quaternion.from_rotation_vector(delta_phi)
q=q*np.prod(delta_q)
"""Estimate gravity from orientation (g)"""
r_prior=quat2rotm(q)
g=r_prior[:,2:3].transpose()*(-EARTH_GRAVITY_MS2) #m/s^2
"""Estimate gravity from acceleration (g_acc)"""
accel_readings=get_acc() #m/s^2
g_acc=accel_readings-lin_acc_prior #m/s^2
"""----------------------------------------------------------Error Model----------------------------------------------------------"""
"Error Model (z)"
z=g-g_acc #m/s^2
"""----------------------------------------------------------Kalman Equations----------------------------------------------------------"""
"""Observation model (H)"""
gx=g[0,0]
gy=g[0,1]
gz=g[0,2]
g_cross=np.array([[0, gz, -gy],[-gz, 0, gx],[gy, -gx, 0]])
H=np.block([g_cross, -kappa*g_cross, I])
"""Innovation covariance (S)"""
S=R+np.dot(H,np.dot(P,H.transpose()))
"""Kalman gain (K)"""
K=np.dot(P,np.dot(H.transpose(),np.linalg.inv(S)))
"""Update error estimate covariance (P+)"""
P=P-np.dot(K,np.dot(H,P))
"""Predict error estimate covariance (P-)"""
D1=np.diag(np.diag(P[0:3,0:3])) #first diagonal block P
D2=np.diag(np.diag(P[3:6,3:6])) #second diagonal block P
D3=np.diag(np.diag(P[6:9,6:9])) #third diagonal block P
Q11=D1+kappa**2*D2+(beta+eta)*I
Q12=(D2+beta*I)
Q12[0,0]*=-kappa
Q22=D2+beta*I
Q33=nu**2*D3+csi*I
Q=np.block([[Q11,Q12,Z],[Q12,Q22,Z],[Z,Z,Q33]])
P=Q
"""Update posterior error (x)"""
x=np.dot(K,z.transpose())
"""----------------------------------------------------------Correct----------------------------------------------------------"""
"""Estimate orientation (q+)"""
theta=x[:3].transpose() #rad
q=q*quaternion.from_rotation_vector(-theta)[0]
"""Estimate linear acceleration (lin_acc_prior)"""
b=x[3:6].transpose() #rad/s
lin_acc_prior = lin_acc_prior*nu-b
"""Estimate gyro offset (gyro_offset)"""
a=x[6:].transpose()
gyro_offset=gyro_offset-a
"""----------------------------------------------------------Compute Angular Velocity----------------------------------------------------------"""
"""Angular velocity (angular_velocity)"""
angular_velocity=np.sum(gyro_readings,axis=0)/N-gyro_offset
隨着我的IMU保持靜止( get_ang
返回值[0,0,0]
和get_acc
返回值[0,0,-9.8]
)我觀察到gyro_offset
異常增長(可能是由於a
值不小)導致在錯誤的g
和z
估計中錯誤計算delta_phi
, delta_q
, q
等。
我檢查了很多次代碼,但沒有發現任何錯誤。 我認為我可以誤解上面鏈接中的指令,可能會混淆度量單位( 度,弧度,m / s ^ 2,g ),但即使嘗試不同的組合,我也會獲得類似的行為。
你能幫我找一下我錯過的東西嗎?
PS可以在每個步驟重現我的設置:
gyro_readings=np.random.normal(0,1,[1,3])/50
accel_readings=np.array([0,0,-9.8])+np.random.normal(0,1,[3])/50
在你在卡爾曼方程下提供的鏈接中,S的轉置被反轉以計算卡爾曼增益。
看起來你在反轉它之前沒有采用S的轉置。 在線:
K=np.dot(P,np.dot(H.transpose(),np.linalg.inv(S)))
它應該是
K=np.dot(P,np.dot(H.transpose(),np.linalg.inv(S.transpose())))
我在您的代碼中看到以下問題:
當從方向矩陣計算g
,你將它乘以地球引力。 結果,您的觀察誤差和創新以m/s2
來衡量。 根據文檔,過濾器需要以units g
為units g
的誤差。 所以我寧願用地球引力划分g_acc
。
當訪問狀態向量x
您使用元素4:6進行線性加速度估計,但這些元素屬於陀螺儀偏移。 元素7:9也是如此,它應該用於加速,而不是用於陀螺儀偏移。
在生成測試信號時,您使用了一些正態分布參數來模擬噪聲。 我將使用您在濾波器實現中使用的完全相同的噪聲參數,否則這兩個噪聲級別彼此不對應,並且濾波器無法以最佳方式執行。
在matlab頁面上給出的Q
的公式與文檔中的原始公式不對應。 比較公式10.1.23
和10.1.24
。 它們分別涉及P元素[0,2:3,5]
和[3,5:3,5]
。 在您的情況下,這意味着子矩陣Q12
不正確。
不幸的是,我無法運行你的python代碼來檢查它是否與上面的建議更好地工作。 但我的matlab代碼顯示了更好的性能。
你可以嘗試一下並發布一些情節嗎?
聲明:本站的技術帖子網頁,遵循CC BY-SA 4.0協議,如果您需要轉載,請注明本站網址或者原文地址。任何問題請咨詢:yoyou2525@163.com.