Modified Rodrigues Parameters Kalman Filter

最新推荐文章于 2024-08-26 21:19:44 发布

原创

最新推荐文章于 2024-08-26 21:19:44 发布 · 置顶 · 813 阅读

3 ·

CC 4.0 BY-SA版权

本文深入探讨了使用扩展卡尔曼滤波器(EKF)算法进行陀螺仪偏置估计的过程，详细解析了系统模型和运动学模型，并通过仿真验证了算法的有效性。

1.系统模型

1.1 陀螺模型

$ω∗=ω+b+ηωb˙=ηb(1)\begin{aligned} \boldsymbol{\omega^{*}} &=\boldsymbol{\omega}+\boldsymbol{b}+\boldsymbol{\eta_{\omega}}\\ \boldsymbol{\dot{b}}&=\boldsymbol{\eta_{b}} \end{aligned}\tag{1}$

其中， $ω\omega$ 是真实的角速度， $b$ 是偏置， $ω∗\omega^{*}$ 是角速度的测量值。 $ηω\eta_{\omega}$ 和 $ηb\eta_{b}$ 是零均值白噪声。

1.2 运动学模型

待估计状态为 $x=[σ,b]T\boldsymbol{x}=[\boldsymbol{\sigma}, \boldsymbol{b}]^{T}$ ，噪声为 $η=[ηω,ηb]T\eta=\left[\boldsymbol{\eta}_{\omega}, \boldsymbol{\eta}_{b}\right]^{T}$ 。
修正罗德里格参数的微分运动学方程为
$σ˙=14B(σ)σ=14[(1−σTσ)I+2σ×+2σσT]ω(2)\dot{\boldsymbol{\sigma}}=\frac{1}{4} \boldsymbol{B}(\boldsymbol{\sigma}) \boldsymbol{\sigma}=\frac{1}{4}\left[\left(1-\boldsymbol{\sigma}^{T} \boldsymbol{\sigma}\right) I+2 \boldsymbol{\sigma}^{\times}+2 \boldsymbol{\sigma} \boldsymbol{\sigma}^{T}\right] \boldsymbol{\omega}\tag{2}$

根据式(2)，可得待估计状态的动力学方程
$x˙=f(x)+g(x,η)(3)\dot\boldsymbol{x}=\boldsymbol{f}(\boldsymbol{x})+\boldsymbol{g}(\boldsymbol{x}, \boldsymbol{\eta})\tag{3}$

其中，
$f(x)=[14[B(σ)](ω∗−b)0]g(x,η)=[−14[B(σ)]ηωηb](4)\begin{aligned} \boldsymbol{f}(\boldsymbol{x}) &=\left[\begin{array}{c} \frac{1}{4}[\boldsymbol{B}(\boldsymbol{\sigma})]\left(\boldsymbol{\omega}^{*}-\boldsymbol{b}\right) \\ \mathbf{0} \end{array}\right] \\ \boldsymbol{g}(\boldsymbol{x}, \boldsymbol{\eta}) &=\left[\begin{array}{c} -\frac{1}{4}[\boldsymbol{B}(\boldsymbol{\sigma})] \boldsymbol{\eta}_{\omega} \\ \boldsymbol{\eta}_{b} \end{array}\right] \end{aligned}\tag{4}$

最低0.47元/天解锁文章