从 MATLAB 到 Python 的错误状态卡尔曼滤波器
Posted
技术标签:
【中文标题】从 MATLAB 到 Python 的错误状态卡尔曼滤波器【英文标题】:Error state Kalman Filter from MATLAB to Python 【发布时间】:2019-06-05 08:28:31 【问题描述】:我正在尝试在 Python 中重现 here 中解释的算法,但我遇到了一些参数奇怪增长的问题。
以下是我的尝试。观察 get_ang()
和 get_acc()
以 degrees/s 返回沿 [x,y,z] 轴的角速度和加速度(但我将此数据转换为 radians/s) 和 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
的值不小)导致在错误计算 delta_phi
、delta_q
、q
等错误估计 g
和 z
。
我检查了很多次代码,但没有发现任何错误。我认为我可能会误解上面链接中的说明,可能会与测量单位(度、弧度、m/s^2、g)混淆,但即使尝试使用不同的组合,我也会得到类似的行为。
您能帮我找出我缺少的东西吗?
附:可以在每个步骤中重现我的设置:
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
【问题讨论】:
我检查了引用的PDF,发现与matlab页面上的描述有很多不匹配的地方。我要纠正所有这些东西,看看它是否有效。 非常感谢,希望您能帮到我:) 这个问题真的很有趣,我会尽量找足够的时间来解决这个问题。到目前为止,我在 matlab 中实现了整个代码,但我观察到的行为与您的相同。 【参考方案1】:在您在卡尔曼方程下提供的链接中,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())))
【讨论】:
谢谢你的回复,但是S是对称的,所以它的转置就是S本身。 我记得我的卡尔曼滤波器存在稳定性问题,有时由于浮点数错误导致 S 和 P 刹车的对称性,也许将这些与它们的转置相加并除以二可以解决您的问题。 再次感谢您,但我检查过,我的计算中没有任何对称性损失。因此,通过您的技巧,我也观察到了相同的行为。【参考方案2】:我在您的代码中看到以下问题:
从方向矩阵计算g
时,您将它乘以地球重力。结果,您的观察错误和创新以m/s2
衡量。根据文档,过滤器需要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 代码显示了更好的性能。
您可以尝试一下并发布一些情节吗?
【讨论】:
我正在尝试实施您的建议。无论如何,我想谈谈你的最后一点:我认为文档中有错误。如您所见,在等式 10.1.23 中,左侧包含 Q_bb,即 P[3,5:3,5](请参见等式 10.1.19),但在右侧,它被 P[0,2 替换: 3,5] 如您所述。我认为这可能是转录错误。你能检查一下这一点吗? 我认为你是对的。可能它们的意思是Theta
和b
不相关,它们的协方差可以忽略不计。我想知道为什么文档和 matlab 描述有这么多不匹配...以上是关于从 MATLAB 到 Python 的错误状态卡尔曼滤波器的主要内容,如果未能解决你的问题,请参考以下文章