Question

我正在尝试重现Python中解释过的here算法，但是由于某些参数的奇怪增长，我遇到了一些问题。以下是我的尝试。观察到get_ang()和get_acc()沿[x，y，z]轴返回的角速度和加速度以 degrees / s 为单位（但我将此数据转换为弧度） / 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和{{1 }}。

我检查了很多次代码，但是没有发现任何错误。我以为我可能会误解上面链接中的指令，可能会与度量单位（度，弧度，m / s ^ 2，g ）混淆，但是即使尝试使用不同的组合，我也会得到类似的结果行为。

您能帮我找到我想念的东西吗？

P.S。可以在每个步骤中重现我的设置：

Answer 1

在您根据卡尔曼方程式提供的链接中，将S的转置取反以计算卡尔曼增益。
似乎您没有在反转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())))

Answer 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代码对它们显示出更好的性能。

您可以尝试并发布一些情节吗？

从MATLAB到Python的错误状态Kalman过滤器

2 个答案: