Unleashing Robotics: Mastering Quaternion Kinematics with Python - Chapter8（原创系列教程）

Unleashing Robotics: Mastering Quaternion Kinematics with Python - Chapter8(原创系列教程)
本系列教程禁止转载，主要是为了有不同见解的同学可以方便联系我，我的邮箱 fanzexuan135@163.com

随机噪声和扰动的积分

我们现在的目标是给出适当的方法来积分动态系统中的随机变量。当然,我们不能积分未知的随机值,但我们可以积分它们的方差和协方差,以便进行不确定性传播。这对于建立连续性质(并以连续时间指定)但以离散方式估计的系统的估计器的协方差矩阵是必需的。

考虑连续时间动态系统:

$$\dot{x}=f(x,u,w)$$

其中$x$是状态向量,$u$是包含噪声$\tilde{u}$的控制信号向量,因此控制测量值为$u_m=u+\tilde{u}$,$w$是随机扰动向量。噪声和扰动都假设为白高斯过程,具体如下:

$$\tilde{u}\sim N(0,U_c),w_c\sim N(0,W_c)$$

其中上标${\bullet }_c$表示连续时间不确定性规范,我们要对其进行积分。

控制信号中的噪声水平$\tilde{u}$和随机扰动$w$之间存在一个重要区别:

• 在离散化时,控制信号在时刻$n\Delta t$被采样,得到$u_{m,n}\equiv u_m(n\Delta t)=u(n\Delta t)+\tilde{u}(n\Delta t)$。测量值的部分显然在整个积分区间内被认为是常数,即$u_m(t)=u_{m,n}$,因此在采样时刻$n\Delta t$的噪声水平也保持不变,

$$\tilde{u}(t)=\tilde{u}(n\Delta t)={\tilde{u}}_n,n\Delta t<t<(n+1)\Delta t$$

• 扰动$w$从不被采样。

因此,这两个随机过程在$\Delta t$上的积分是不同的。让我们来研究一下。

连续时间误差状态动力学可以线性化为:

$$\delta \dot{x}=A\delta x+B\tilde{u}+Cw$$

其中

$$A\equiv \frac{\partial f}{\partial \delta x}{|}_{x,u_m},B\equiv \frac{\partial f}{\partial \tilde{u}}{|}_{x,u_m},C\equiv \frac{\partial f}{\partial w}{|}_{x,u_m}$$

并在采样周期$\Delta t$上积分,得到:

$$\begin{array}{rl}\delta x_{n+1}& =\delta x_n+{\int }_{n\Delta t}^{(n+1)\Delta t}(A\delta x(\tau )+B\tilde{u}(\tau )+C{w}_c(\tau ))d\tau \\ & =\delta x_n+{\int }_{n\Delta t}^{(n+1)\Delta t}A\delta x(\tau )d\tau +{\int }_{n\Delta t}^{(n+1)\Delta t}B\tilde{u}(\tau )d\tau +{\int }_{n\Delta t}^{(n+1)\Delta t}C{w}_c(\tau )d\tau \end{array}$$

这有三个性质非常不同的项。可以按如下方式进行积分:

1. 从前面的章节我们知道动态部分的积分给出了转移矩阵,

$$\delta x_n+{\int }_{n\Delta t}^{(n+1)\Delta t}A\delta x(\tau )d\tau =\Phi \cdot \delta x_n$$

其中$\Phi =e^{A\Delta t}$可以用闭式计算或以不同的精度级别近似。

2. 我们有

$${\int }_{n\Delta t}^{(n+1)\Delta t}B\tilde{u}(\tau )d\tau =B\Delta t{\tilde{u}}_n$$

这意味着一旦被采样,测量噪声就以确定的方式被积分,因为它在积分区间内的行为是已知的。

3. 从概率论我们知道,在周期$\Delta t$上积分连续白高斯噪声会产生一个离散白高斯脉冲$w_n$,表示为:

$$w_n\equiv {\int }_{n\Delta t}^{(n+1)\Delta t}w(\tau )d\tau ,w_n\sim N(0,W),\text{with }W=W_c\Delta t$$

我们注意到,与上面的测量噪声相反,扰动在积分区间内没有确定的行为,因此必须以随机方式积分。

因此,离散时间误差状态动态系统可以写成:

$$\delta x_{n+1}=F_x\delta x_n+F_u{\tilde{u}}_n+F_w{w}_n$$

其中转移矩阵、控制矩阵和扰动矩阵分别为:

$$F_x=\Phi =e^{A\Delta t},F_u=B\Delta t,F_w=C$$

噪声和扰动水平定义为:

$${\tilde{u}}_n\sim N(0,U),w_n\sim N(0,W)$$

其中

$$U=U_c,W=W_c\Delta t$$

EKF的预测阶段将根据以下方式传播误差状态的均值和协方差矩阵:

$$\begin{array}{rl}{\widehat{\delta x}}_{n+1}& =F_x{\widehat{\delta x}}_n\\ P_{n+1}& =F_x{P}_n{F}_x^{\top }+F_{u}U{F}_u^{\top }+F_{w}W{F}_w^{\top }\\ & =e^{A\Delta t}{P}_n(e^{A\Delta t}{)}^{\top }+\Delta t^{2}B{U}_c{B}^{\top }+\Delta tC{W}_c{C}^{\top }\end{array}$$

在这里观察积分区间$\Delta t$对协方差更新(440)的三项的不同影响是很重要和有启发性的:动态误差项是指数的,测量误差项是二次的,扰动误差项是线性的。

噪声和扰动脉冲

人们经常遇到(例如在重用现有代码或解释其他作者的文档时)EKF预测方程的一个更简单的形式,即:

$$P_{n+1}=F_x{P}_n{F}_x^{\top }+Q$$

这对应于一般的离散时间动态系统:

$$\delta x_{n+1}=F_x\delta x_n+i$$

其中

$$i\sim N(0,Q)$$

是直接添加到时刻$t_{n+1}$的状态向量中的随机(白噪声,高斯)脉冲向量。矩阵$Q$被简单地视为脉冲协方差矩阵。从我们所看到的,我们应该如下计算这个协方差矩阵:

$$Q=\Delta t^{2}B{U}_c{B}^{\top }+\Delta tC{W}_c{C}^{\top }$$

在脉冲不影响整个状态的情况下,这种情况经常发生,矩阵$Q$不是全对角的,并且可能包含大量的零。然后可以写出等价形式:

$$\delta x_{n+1}=F_x\delta x_n+F_{i}i$$

其中

$$i\sim N(0,Q_i)$$

矩阵$F_i$只是将每个单独的脉冲映射到它所影响的状态向量的部分。相关的协方差$Q_i$则更小,并且是全对角的。请参考下一节的示例。在这种情况下,ESKF时间更新变为:

$$\begin{array}{rl}{\widehat{\delta x}}_{n+1}& =F_x{\widehat{\delta x}}_n\\ P_{n+1}& =F_x{P}_n{F}_x^{\top }+F_i{Q}_i{F}_i^{\top }\end{array}$$

显然,所有这些形式都是等价的,这可以从一般扰动$Q$的以下双重等式中看出:

$$F_i{Q}_i{F}_i^{\top }=Q=\Delta t^{2}B{U}_c{B}^{\top }+\Delta tC{W}_c{C}^{\top }$$

完整IMU示例

我们研究为IMU构建误差状态卡尔曼滤波器。误差状态系统在下列方程中定义，涉及标称状态$\mathbf{x}$,误差状态$\delta \mathbf{x}$,有噪声的控制信号${\mathbf{u}}_m=\mathbf{u}+\tilde{\mathbf{u}}$和扰动$\mathbf{w}$,具体定义为:

$$\mathbf{x}=\left[\begin{array}{c}\mathbf{p}\\ \mathbf{v}\\ \mathbf{q}\\ {\mathbf{a}}_b\\ {\mathbf{\omega }}_b\\ \mathbf{g}\end{array}\right],\delta \mathbf{x}=\left[\begin{array}{c}\delta \mathbf{p}\\ \delta \mathbf{v}\\ \delta \mathbf{\theta }\\ \delta {\mathbf{a}}_b\\ \delta {\mathbf{\omega }}_b\\ \delta \mathbf{g}\end{array}\right],{\mathbf{u}}_m=\left[\begin{array}{c}{\mathbf{a}}_m\\ {\mathbf{\omega }}_m\end{array}\right],\tilde{\mathbf{u}}=\left[\begin{array}{c}\tilde{\mathbf{a}}\\ \tilde{\mathbf{\omega }}\end{array}\right],\mathbf{w}=\left[\begin{array}{c}{\mathbf{a}}_w\\ {\mathbf{\omega }}_w\end{array}\right]$$

在这里:

• $\mathbf{p}$, $\mathbf{v}$, $\mathbf{q}$ 分别是位置、速度和姿态(四元数)的标称状态。
• ${\mathbf{a}}_b$, ${\mathbf{\omega }}_b$ 是加速度计和陀螺仪偏差的标称状态。
• $\mathbf{g}$ 是重力向量的标称状态。
• $\delta \mathbf{p}$, $\delta \mathbf{v}$, $\delta \mathbf{\theta }$ 分别是位置、速度和姿态(欧拉角)误差状态。
• $\delta {\mathbf{a}}_b$, $\delta {\mathbf{\omega }}_b$, $\delta \mathbf{g}$ 分别是加速度计偏差、陀螺仪偏差和重力向量的误差状态。
• ${\mathbf{a}}_m$, ${\mathbf{\omega }}_m$ 是加速度计和陀螺仪的测量值。
• $\tilde{\mathbf{a}}$, $\tilde{\mathbf{\omega }}$ 是加速度计和陀螺仪的测量噪声。
• ${\mathbf{a}}_w$, ${\mathbf{\omega }}_w$ 是加速度计和陀螺仪偏差的过程噪声。
• ${\mathbf{a}}_n=\tilde{\mathbf{a}}$, ${\mathbf{\omega }}_n=\tilde{\mathbf{\omega }}$ 是加速度计和陀螺仪的(重新定义的)测量噪声。

注意,这里我们使用了不同的符号来区分标称状态、误差状态、测量值和噪声。另外,加速度计和陀螺仪的测量噪声最初被定义为$\tilde{\mathbf{a}}$和$\tilde{\mathbf{\omega }}$,但在后面的推导中被重新定义为${\mathbf{a}}_n$和${\mathbf{\omega }}_n$,以简化公式。

$$\begin{array}{rl}x& =[p^{\top },v^{\top },q^{\top },a_b^{\top },{\omega }_b^{\top },g^{\top }{]}^{\top }\\ \delta x& =[\delta p^{\top },\delta v^{\top },\delta {\theta }^{\top },\delta a_b^{\top },\delta {\omega }_b^{\top },\delta g^{\top }{]}^{\top }\\ u_m& =[a_m^{\top },{\omega }_m^{\top }{]}^{\top }\\ \tilde{u}& =[{\tilde{a}}^{\top },{\tilde{\omega }}^{\top }{]}^{\top }\\ w& =[a_w^{\top },{\omega }_w^{\top }{]}^{\top }\end{array}$$

在我们正在考虑的IMU模型中,控制噪声对应于IMU测量中的附加噪声。扰动影响偏差,从而产生它们的随机游走行为。动态矩阵、控制矩阵和扰动矩阵为(见(428)、(378)和(238)):

$$A=\left[\begin{array}{cccccc}0& P_v& 0& 0& 0& 0\\ 0& 0& V_{\theta }& V_a& 0& V_g\\ 0& 0& {\Theta }_{\theta }& 0& {\Theta }_{\omega }& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right],B=\left[\begin{array}{cc}0& 0\\ -R& 0\\ 0& -I\\ 0& 0\\ 0& 0\\ 0& 0\end{array}\right],C=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ I& 0\\ 0& I\\ 0& 0\end{array}\right]$$

在IMU的三个轴上具有相同类型的加速度计和陀螺仪三元组的常规情况下,噪声和扰动是各向同性的。它们的标准差指定为标量,如下所示:

$${\sigma }_{\tilde{a}}[{\text{m/s}}^2],{\sigma }_{\tilde{\omega }}[\text{rad/s}],{\sigma }_{a_w}[{\text{m/s}}^2\sqrt{\text{s}}],{\sigma }_{{\omega }_w}[\text{rad/s}\sqrt{\text{s}}]$$

它们的协方差矩阵是纯对角的,得到:

$$U_c=\left[\begin{array}{cc}{\sigma }_{\tilde{a}}^{2}I& 0\\ 0& {\sigma }_{\tilde{\omega }}^{2}I\end{array}\right],W_c=\left[\begin{array}{cc}{\sigma }_{a_w}^{2}I& 0\\ 0& {\sigma }_{{\omega }_w}^{2}I\end{array}\right]$$

系统以$\Delta t$的采样度量随时间演化,遵循以下离散时间动力学方程:

$$\delta {\mathbf{x}}_{n+1}={\mathbf{F}}_x\delta {\mathbf{x}}_n+{\mathbf{F}}_u{\tilde{\mathbf{u}}}_n+{\mathbf{F}}_w{\mathbf{w}}_n\text{\hspace{1em}(435)}$$

其中,转移矩阵${\mathbf{F}}_x$,控制矩阵${\mathbf{F}}_u$和扰动矩阵${\mathbf{F}}_w$分别为:

$${\mathbf{F}}_x=\mathbf{\Phi }=e^{\mathbf{A}\Delta t},{\mathbf{F}}_u=\mathbf{B}\Delta t,{\mathbf{F}}_w=\mathbf{C}\text{\hspace{1em}(436)}$$

噪声和扰动水平定义为:

$${\tilde{\mathbf{u}}}_n\sim \mathcal{N}(0,\mathbf{U}),{\mathbf{w}}_n\sim \mathcal{N}(0,\mathbf{W})\text{\hspace{1em}(437)}$$

其中:

$$\mathbf{U}={\mathbf{U}}_c,\mathbf{W}={\mathbf{W}}_c\Delta t\text{\hspace{1em}(438)}$$

转移矩阵${\mathbf{F}}_x=\mathbf{\Phi }$可以通过多种方式计算,例如:

1. 使用矩阵指数的定义直接计算:

$$\mathbf{\Phi }=e^{\mathbf{A}\Delta t}=\mathbf{I}+\mathbf{A}\Delta t+\frac{1}{2!}{\mathbf{A}}^2\Delta t^2+\frac{1}{3!}{\mathbf{A}}^3\Delta t^3+\cdots$$

2. 对于某些特殊形式的$\mathbf{A}$(例如,当它是对角或块对角矩阵时),矩阵指数可能有封闭形式的解。例如,如果$\mathbf{A}=\mathrm{diag}({\lambda }_1,{\lambda }_2,\dots ,{\lambda }_n)$,那么

$$e^{\mathbf{A}\Delta t}=\mathrm{diag}(e^{{\lambda }_1\Delta t},e^{{\lambda }_2\Delta t},\dots ,e^{{\lambda }_n\Delta t})$$

3. 使用数值方法,如帕德近似(Padé approximation)或缩放和求幂(scaling and squaring)方法。这些方法对于一般形式的$\mathbf{A}$很有用,并且可以在许多数值计算库中找到,如scipy.linalg.expm。

4. 对于一些特定问题,转移矩阵$\mathbf{\Phi }$可能有封闭形式的解。这通常是通过求解微分方程$\dot{\mathbf{x}}=\mathbf{Ax}$来获得的。例如,对于前面讨论的简化IMU模型,旋转部分的转移矩阵有封闭解:

$${\mathbf{\Phi }}_{\theta \theta }=\exp ({\mathbf{A}}_{\theta \theta }\Delta t)=\mathbf{I}-[\mathbf{\omega }{]}_{\times }\sin \Vert \mathbf{\omega }\Vert \Delta t+[\mathbf{\omega }{]}_{\times }^2(1-\cos \Vert \mathbf{\omega }\Vert \Delta t)$$

其中$\mathbf{\omega }$是角速度向量。

在实际应用中,应根据问题的具体特征和计算效率的要求选择合适的方法来计算转移矩阵。封闭解通常是最可取的,因为它们精确且计算效率高,但它们并不总是可用的。当封闭解不存在时,数值方法提供了一个很好的折衷方案。。

噪声和扰动脉冲

在以脉冲$i$的形式指定扰动的情况下,我们可以重新定义我们的系统如下:

$$\delta x_{n+1}=F_x(x_n,u_m)\cdot \delta x_n+F_i\cdot i$$

其中标称状态、误差状态、控制和脉冲向量定义为:

$$\begin{array}{rl}x& =[p^{\top },v^{\top },q^{\top },a_b^{\top },{\omega }_b^{\top },g^{\top }{]}^{\top }\\ \delta x& =[\delta p^{\top },\delta v^{\top },\delta {\theta }^{\top },\delta a_b^{\top },\delta {\omega }_b^{\top },\delta g^{\top }{]}^{\top }\\ u_m& =[a_m^{\top },{\omega }_m^{\top }{]}^{\top }\\ i& =[v_i^{\top },{\theta }_i^{\top },a_i^{\top },{\omega }_i^{\top }{]}^{\top }\end{array}$$

转移矩阵和扰动矩阵定义为:

$$F_x=\Phi =e^{A\Delta t},F_i=\left[\begin{array}{cccc}0& 0& 0& 0\\ I& 0& 0& 0\\ 0& I& 0& 0\\ 0& 0& I& 0\\ 0& 0& 0& I\\ 0& 0& 0& 0\end{array}\right]$$

脉冲方差指定为:

$$i\sim N(0,Q_i),Q_i=\left[\begin{array}{cccc}{\sigma }_{\tilde{a}}^2\Delta t^{2}I& & & \\ & {\sigma }_{\tilde{\omega }}^2\Delta t^{2}I& & \\ & & {\sigma }_{a_w}^2\Delta tI& \\ & & & {\sigma }_{{\omega }_w}^2\Delta tI\end{array}\right]$$

我们当然可以使用全状态扰动脉冲:

$$\delta x_{n+1}=F_x(x_n,u_m)\cdot \delta x_n+i$$

其中

$$i=\left[\begin{array}{c}0\\ v_i\\ {\theta }_i\\ a_i\\ {\omega }_i\\ 0\end{array}\right],i\sim N(0,Q),Q=\left[\begin{array}{ccccc}0& & & & \\ {\sigma }_{\tilde{a}}^2\Delta t^{2}I& & & & \\ & {\sigma }_{\tilde{\omega }}^2\Delta t^{2}I& & & \\ & & {\sigma }_{a_w}^2\Delta tI& & \\ & & & {\sigma }_{{\omega }_w}^2\Delta tI& \\ & & & & 0\end{array}\right]$$

【代码示例】

下面是一个使用Python的示例代码,说明如何在IMU积分中应用上述噪声和扰动脉冲的概念:

import numpy as np

def imu_predict(x_prev, P_prev, a_m, w_m, dt, Q_i):
    """使用噪声和扰动脉冲进行IMU预测步骤。
    
    Args:
        x_prev: 上一时刻的状态估计值,形状为(16,)。
        P_prev: 上一时刻的状态协方差矩阵,形状为(15, 15)。
        a_m: 当前加速度计测量值,形状为(3,)。
        w_m: 当前陀螺仪测量值,形状为(3,)。
        dt: 采样时间。
        Q_i: 脉冲噪声协方差矩阵,形状为(12, 12)。

    Returns:
        x_pred: 当前时刻的状态预测值,形状为(16,)。  
        P_pred: 当前时刻的状态协方差预测值,形状为(15, 15)。
    """
    # 提取状态变量
    p, v, q, a_b, w_b, g = np.split(x_prev, [3, 6, 10, 13, 16])
    
    # 标称状态积分
    R = quat_to_rot(q)
    a = R.dot(a_m - a_b) + g
    w = w_m - w_b
    p_pred = p + v*dt + 0.5*a*dt**2
    v_pred = v + a*dt
    q_pred = quat_multiply(q, vec_to_quat(w*dt))
    
    # 误差状态转移矩阵
    F_x = np.eye(15)
    F_x[0:3, 3:6] = np.eye(3) * dt
    F_x[3:6, 6:9] = -skew(a_m - a_b) * dt
    F_x[3:6, 9:12] = -R * dt
    F_x[3:6, 12:15] = np.eye(3) * dt
    F_x[6:9, 6:9] = np.eye(3) - skew(w*dt)
    
    # 脉冲噪声矩阵  
    F_i = np.zeros((15, 12)) 
    F_i[3:6, 0:3] = np.eye(3)
    F_i[6:9, 3:6] = np.eye(3)
    F_i[9:12, 6:9] = np.eye(3)
    F_i[12:15, 9:12] = np.eye(3)
    
    # 误差状态协方差预测
    P_pred = F_x.dot(P_prev).dot(F_x.T) + F_i.dot(Q_i).dot(F_i.T) * dt
    
    # 组装预测状态向量
    x_pred = np.concatenate([p_pred, v_pred, q_pred, a_b, w_b, g])
    
    return x_pred, P_pred

这个函数实现了IMU误差状态系统的离散时间预测步骤。它接受上一时刻的状态估计值和协方差矩阵,当前IMU测量值,采样时间以及脉冲噪声协方差矩阵作为输入。

函数首先提取各个状态变量,并使用它们进行标称状态的积分。这里使用了简单的欧拉积分公式:

$$\begin{array}{rl}{\mathbf{p}}_k& ={\mathbf{p}}_{k-1}+{\mathbf{v}}_{k-1}\Delta t+\frac{1}{2}{\mathbf{a}}_{k-1}\Delta t^2\\ {\mathbf{v}}_k& ={\mathbf{v}}_{k-1}+{\mathbf{a}}_{k-1}\Delta t\\ {\mathbf{q}}_k& ={\mathbf{q}}_{k-1}\otimes \left[\begin{array}{c}\cos (\frac{1}{2}\Vert {\mathbf{w}}_{k-1}\Vert \Delta t)\\ \frac{{\mathbf{w}}_{k-1}}{\Vert {\mathbf{w}}_{k-1}\Vert }\sin (\frac{1}{2}\Vert {\mathbf{w}}_{k-1}\Vert \Delta t)\end{array}\right]\end{array}$$

其中${\mathbf{a}}_{k-1}={\mathbf{R}}_{k-1}({\mathbf{a}}_m-{\mathbf{a}}_b)+\mathbf{g}$,${\mathbf{w}}_{k-1}={\mathbf{w}}_m-{\mathbf{w}}_b$,而${\mathbf{R}}_{k-1}$是由四元数${\mathbf{q}}_{k-1}$表示的旋转矩阵。

然后,它计算误差状态转移矩阵${\mathbf{F}}_x$:

$${\mathbf{F}}_x=\left[\begin{array}{cccccc}{\mathbf{I}}_3& {\mathbf{I}}_3\Delta t& 0& 0& 0& 0\\ 0& {\mathbf{I}}_3& -[{\mathbf{a}}_m-{\mathbf{a}}_b{]}_{\times }\Delta t& -\mathbf{R}\Delta t& 0& {\mathbf{I}}_3\Delta t\\ 0& 0& {\mathbf{I}}_3-[\mathbf{w}{]}_{\times }\Delta t& 0& -{\mathbf{I}}_3\Delta t& 0\\ 0& 0& 0& {\mathbf{I}}_3& 0& 0\\ 0& 0& 0& 0& {\mathbf{I}}_3& 0\\ 0& 0& 0& 0& 0& {\mathbf{I}}_3\end{array}\right]$$

和脉冲噪声矩阵${\mathbf{F}}_i$:

$${\mathbf{F}}_i=\left[\begin{array}{cccc}0& 0& 0& 0\\ {\mathbf{I}}_3& 0& 0& 0\\ 0& {\mathbf{I}}_3& 0& 0\\ 0& 0& {\mathbf{I}}_3& 0\\ 0& 0& 0& {\mathbf{I}}_3\\ 0& 0& 0& 0\end{array}\right]$$

最后,根据以下公式,使用${\mathbf{F}}_x$,${\mathbf{F}}_i$和${\mathbf{Q}}_i$计算新的误差状态协方差矩阵:

$${\mathbf{P}}_k={\mathbf{F}}_x{\mathbf{P}}_{k-1}{\mathbf{F}}_x^{\top }+{\mathbf{F}}_i{\mathbf{Q}}_i{\mathbf{F}}_i^{\top }\Delta t$$

预测的状态向量通过组装标称状态积分和先前的偏差估计值来构建:

$${\hat{\mathbf{x}}}_k=\left[\begin{array}{c}{\mathbf{p}}_k\\ {\mathbf{v}}_k\\ {\mathbf{q}}_k\\ {\mathbf{a}}_b\\ {\mathbf{w}}_b\\ \mathbf{g}\end{array}\right]$$

这个函数可以作为基于EKF的IMU积分器的一部分被调用。在每个IMU测量值到达时,它执行预测步骤,然后可以使用其他传感器数据(如GPS或视觉测量值)进行校正步骤。通过适当地建模IMU噪声和扰动的影响,这种方法可以提供准确和稳健的状态估计。

这里使用的是脉冲噪声模型,其中噪声被假设为离散时间脉冲。这通常是IMU噪声的一个很好的近似,因为IMU测量本身是在离散时间点上进行的。然而,对于其他类型的连续时间噪声,可能需要使用不同的积分方法。

总之,将随机噪声和扰动正确整合到状态估计框架中,对于获得准确和可靠的结果至关重要。通过仔细分析噪声的性质,并使用适当的数学工具对其进行建模,我们可以开发出强大的算法,即使在存在不确定性的情况下也能很好地工作。

Q&A 如何在离散时间状态估计框架(如卡尔曼滤波)中正确地处理连续时间噪声和扰动的问题

在许多实际系统中,状态的动力学是以连续时间的形式给出的,例如微分方程:

$$\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x},\mathbf{u},\mathbf{w})$$

其中$\mathbf{x}$是状态向量,$\mathbf{u}$是控制输入,$\mathbf{w}$是过程噪声。然而,状态估计通常是在离散时间点上进行的,因为测量值只在特定的时刻可用。这就要求我们将连续时间的动力学方程转换为离散时间的形式:

$${\mathbf{x}}_{k+1}={\mathbf{x}}_k+{\int }_{t_k}^{t_{k+1}}\mathbf{f}(\mathbf{x}(\tau ),\mathbf{u}(\tau ),\mathbf{w}(\tau ))d\tau$$

问题在于,上述积分涉及随机过程$\mathbf{w}(\tau )$,它在不同时刻的值是相关的。因此,我们不能简单地将$\mathbf{w}(\tau )$在积分区间内的均值作为离散时间噪声${\mathbf{w}}_k$。

这一部分的主要数学结论是:

1. 对于控制输入$\mathbf{u}$中的噪声$\tilde{\mathbf{u}}$,由于它在每个采样时刻都被采样并在整个积分区间内保持不变,因此它可以像确定性输入一样被处理:

   $${\int }_{t_k}^{t_{k+1}}\mathbf{B}\tilde{\mathbf{u}}(\tau )d\tau =\mathbf{B}\Delta t\cdot {\tilde{\mathbf{u}}}_k$$

   其中$\mathbf{B}$是控制矩阵,$\Delta t=t_{k+1}-t_k$是采样间隔。

2. 对于过程噪声$\mathbf{w}$,由于它在整个积分区间内都是随机变化的,因此它必须在积分意义上进行处理。积分一个连续时间白噪声过程在时间间隔$\Delta t$上得到一个离散时间高斯随机变量:

   $${\mathbf{w}}_k={\int }_{t_k}^{t_{k+1}}\mathbf{w}(\tau )d\tau ,{\mathbf{w}}_k\sim \mathcal{N}(0,{\mathbf{Q}}_k),{\mathbf{Q}}_k={\int }_{t_k}^{t_{k+1}}{\mathbf{Q}}_c(\tau )d\tau \approx {\mathbf{Q}}_c\Delta t$$

   其中${\mathbf{Q}}_c$是连续时间噪声的功率谱密度矩阵。

基于这些结论,我们可以将含有噪声和扰动的连续时间状态方程:

$$\delta \dot{\mathbf{x}}=\mathbf{A}\delta \mathbf{x}+\mathbf{B}\tilde{\mathbf{u}}+\mathbf{Cw}$$

转换为等价的离散时间方程:

$$\delta {\mathbf{x}}_{k+1}={\mathbf{F}}_x\delta {\mathbf{x}}_k+{\mathbf{F}}_u{\tilde{\mathbf{u}}}_k+{\mathbf{F}}_w{\mathbf{w}}_k$$

其中:

$${\mathbf{F}}_x=e^{\mathbf{A}\Delta t},{\mathbf{F}}_u=\mathbf{B}\Delta t,{\mathbf{F}}_w=\mathbf{C}$$

这就是我们在IMU误差状态动力学推导中看到的形式。

让我们考虑一个简单的例子。假设我们有一个标量状态$x$,它的动力学由下式给出:

$$\dot{x}=ax+bu+w$$

其中$a$和$b$是常数,$u$是控制输入,$w$是白噪声过程,其功率谱密度为$q_c$。

根据上述结论,我们可以得到等价的离散时间状态方程:

$$x_{k+1}=e^{a\Delta t}{x}_k+\frac{b}{a}(e^{a\Delta t}-1)u_k+w_k$$

其中离散时间噪声$w_k\sim \mathcal{N}(0,q_c\Delta t)$。

在卡尔曼滤波的预测步骤中,我们需要计算状态的先验估计及其协方差:

$${\hat{x}}_{k+1|k}=e^{a\Delta t}{\hat{x}}_{k|k}+\frac{b}{a}(e^{a\Delta t}-1)u_k$$

$$P_{k+1|k}=e^{a\Delta t}{P}_{k|k}{e}^{a\Delta t}+q_c\Delta t$$

可以看到,离散时间噪声的协方差$q_c\Delta t$直接加到了状态协方差的更新公式中。这正是由于我们正确地处理了连续时间噪声的积分。

总之,"随机噪声和扰动的积分"这一部分提供了一个理论基础,让我们能够在离散时间状态估计器中正确地处理连续时间噪声。这对于许多实际系统来说是非常重要的,因为忽略或错误处理噪声的积分会导致次优的估计性能。IMU误差状态卡尔曼滤波就是一个典型的例子,它需要在离散时间框架内处理陀螺仪和加速度计的连续时间噪声。