Unleashing Robotics: Mastering Quaternion Kinematics with Python - Chapter7(原创系列教程)
本系列教程禁止转载，主要是为了有不同见解的同学可以方便联系我，我的邮箱 [email protected]
7. 使用截断级数的近似方法
在状态估计问题中,我们通常使用一个称为状态转移矩阵(或简称转移矩阵)的矩阵将系统状态从一个时间点传播到下一个时间点。这个矩阵本质上描述了系统状态如何随时间演化。
对于线性系统,状态转移是精确的,可以用矩阵指数表示:
x ( t + Δ t ) = e A Δ t x ( t ) \mathbf{x}(t + \Delta t) = e^{\mathbf{A}\Delta t}\mathbf{x}(t) x(t+Δt)=eAΔtx(t)
其中 A \mathbf{A} A 是系统动力学矩阵, Δ t \Delta t Δt 是时间步长。这里,矩阵指数 e A Δ t e^{\mathbf{A}\Delta t} eAΔt 就是转移矩阵。
然而,对于非线性系统,精确的转移矩阵通常难以计算。这就是近似方法发挥作用的地方。我们将提到的技术,即泰勒级数截断,是一种近似计算转移矩阵的方法。
泰勒级数是一种用多项式近似函数的方法。对于矩阵指数,泰勒级数展开如下:
e A Δ t = I + A Δ t + 1 2 ! ( A Δ t ) 2 + 1 3 ! ( A Δ t ) 3 + ⋯ e^{\mathbf{A}\Delta t} = \mathbf{I} + \mathbf{A}\Delta t + \frac{1}{2!}(\mathbf{A}\Delta t)^2 + \frac{1}{3!}(\mathbf{A}\Delta t)^3 + \cdots eAΔt=I+AΔt+2!1(AΔt)2+3!1(AΔt)3+⋯
我们提到的截断泰勒级数的方法,本质上是取这个无限级数的前几项作为近似。例如,如果我们只取前两项,我们得到:
e A Δ t ≈ I + A Δ t e^{\mathbf{A}\Delta t} \approx \mathbf{I} + \mathbf{A}\Delta t eAΔt≈I+AΔt
这就是所谓的一阶近似。我们可以取更多的项来提高精度,但计算成本也会增加。
现在,关于系统范围和块范围截断的区别:
-
系统范围截断是对整个转移矩阵应用相同的截断。也就是说,我们对矩阵中的每个元素使用相同阶数的近似。
-
块范围截断是对转移矩阵的不同部分(或块)应用不同的截断。例如,如果我们的状态向量包括位置和速度,我们可能对位置部分使用二阶近似,而对速度部分使用一阶近似。
块范围截断的优点是,它允许我们对状态的不同部分使用不同的精度,这在计算效率和精度之间提供了更多的灵活性。
总之,泰勒级数截断是一种用于近似非线性系统状态转移矩阵的技术。它通过将矩阵指数的无限泰勒级数截断为有限的项来工作。这种截断可以在整个矩阵上统一应用(系统范围),也可以在矩阵的不同部分应用不同的截断(块范围)。这种技术让我们可以在计算效率和近似精度之间进行权衡,这在实时系统中是非常宝贵的。
在上一节中,我们为复杂的、IMU驱动的动态系统(以线性化的、误差状态形式 δ x ˙ = A δ x \dot{\delta\mathbf{x}} = \mathbf{A}\delta\mathbf{x} δx˙=Aδx 编写)的转移矩阵设计了闭合形式的表达式。闭合形式的表达式可能总是令人感兴趣的,但目前尚不清楚我们应该在多大程度上关注高阶误差及其对实际算法性能的影响。这一点在以相对高速率观测(从而补偿)IMU积分误差的系统中尤其值得注意,例如视觉惯性或GPS惯性融合方案。
在本节中,我们设计了用于近似转移矩阵的方法。它们从相同的假设开始,即转移矩阵可以表示为泰勒级数,然后在最重要的项处截断这些级数。这种截断可以在系统范围内进行,也可以在块范围内进行。
7.1 系统范围内的截断
7.1.1 一阶截断:有限差分法
对于类型为 x ˙ = f ( t , x ) \dot{\mathbf{x}} = \mathbf{f}(t, \mathbf{x}) x˙=f(t,x) 的系统,一种典型的、广泛使用的积分方法基于有限差分法来计算导数,即
x ˙ ≜ lim δ t → 0 x ( t + δ t ) − x ( t ) δ t ≈ x n + 1 − x n Δ t \dot{\mathbf{x}} \triangleq \lim_{\delta t \to 0} \frac{\mathbf{x}(t + \delta t) - \mathbf{x}(t)}{\delta t} \approx \frac{\mathbf{x}_{n+1} - \mathbf{x}_n}{\Delta t} x˙≜δt→0limδtx(t+δt)−x(t)≈Δtxn+1−xn
这立即导致
x n + 1 ≈ x n + Δ t f ( t n , x n ) \mathbf{x}_{n+1} \approx \mathbf{x}_n + \Delta t \mathbf{f}(t_n, \mathbf{x}_n) xn+1≈xn+Δtf(tn,xn)
这正是欧拉方法。在积分区间开始时对函数 f ( ) \mathbf{f}() f() 进行线性化,得到
x n + 1 ≈ x n + Δ t A x n \mathbf{x}_{n+1} \approx \mathbf{x}_n + \Delta t \mathbf{A} \mathbf{x}_n xn+1≈xn+ΔtAxn
其中 A ≜ ∂ f ∂ x ( t n , x n ) \mathbf{A} \triangleq \frac{\partial\mathbf{f}}{\partial\mathbf{x}}(t_n, \mathbf{x}_n) A≜∂x∂f(tn,xn) 是雅可比矩阵。这与为线性化的微分方程编写指数解并在线性项处截断级数是严格等价的(即,以下关系与前一个相同),
x n + 1 = e A Δ t x n ≈ ( I + Δ t A ) x n \mathbf{x}_{n+1} = e^{\mathbf{A}\Delta t}\mathbf{x}_n \approx (\mathbf{I} + \Delta t \mathbf{A}) \mathbf{x}_n xn+1=eAΔtxn≈(I+ΔtA)xn
这意味着欧拉方法、有限差分法和一阶系统范围泰勒截断法都是相同的。我们得到近似的转移矩阵,
Φ ≈ I + Δ t A \mathbf{\Phi} \approx \mathbf{I} + \Delta t\mathbf{A} Φ≈I+ΔtA
对于6.2节中的简化IMU示例,有限差分法得到近似转移矩阵
Φ ≈ [ I I Δ t 0 0 I − R [ a ] × Δ t 0 0 I − [ ω Δ t ] × ] \mathbf{\Phi} \approx \begin{bmatrix} \mathbf{I} & \mathbf{I}\Delta t & \mathbf{0} \\ \mathbf{0} & \mathbf{I} & -\mathbf{R}[\mathbf{a}]_\times \Delta t \\ \mathbf{0} & \mathbf{0} & \mathbf{I} - [\boldsymbol{\omega}\Delta t]_\times \end{bmatrix} Φ≈ I00IΔtI00−R[a]×ΔtI−[ωΔt]×
然而,我们已经从6.1节知道,旋转项有一个紧凑的、封闭形式的解,即 Φ θ θ = R ( ω Δ t ) ⊤ \mathbf{\Phi}_{\theta\theta} = \mathbf{R}(\boldsymbol{\omega}\Delta t)^\top Φθθ=R(ωΔt)⊤。重写转移矩阵以反映这一点是很方便的,
Φ
≈
[
I
I
Δ
t
0
0
I
−
R
[
a
]
×
Δ
t
0
0
R
{
ω
Δ
t
}
⊤
]
\mathbf{\Phi} \approx \begin{bmatrix} \mathbf{I} & \mathbf{I}\Delta t & \mathbf{0} \\ \mathbf{0} & \mathbf{I} & -\mathbf{R}[\mathbf{a}]_\times \Delta t \\ \mathbf{0} & \mathbf{0} & \mathbf{R}\{\boldsymbol{\omega}\Delta t\}^\top \end{bmatrix}
Φ≈
I00IΔtI00−R[a]×ΔtR{ωΔt}⊤
这个结论的意义在于,它为转移矩阵的旋转部分提供了一个精确的解,而不需要使用泰勒级数近似。这在实践中非常有用,因为旋转在许多状态估计问题中起着关键作用,使用封闭形式的解可以提高数值稳定性和精度。
让我们以一个简单的3D姿态估计问题为例。假设我们的状态向量由姿态(表示为旋转矩阵)和角速度组成:
x = [ R ω ] \mathbf{x} = \begin{bmatrix} \mathbf{R} \\ \boldsymbol{\omega} \end{bmatrix} x=[Rω]
系统的动力学方程为:
R ˙ = R [ ω ] × ω ˙ = 0 \begin{aligned} \dot{\mathbf{R}} &= \mathbf{R}[\boldsymbol{\omega}]_\times \\ \dot{\boldsymbol{\omega}} &= \mathbf{0} \end{aligned} R˙ω˙=R[ω]×=0
其中 [ ω ] × [\boldsymbol{\omega}]_\times [ω]× 是角速度的叉积矩阵。
对这个系统进行离散化,我们得到:
R k + 1 = R k Φ θ θ ω k + 1 = ω k \begin{aligned} \mathbf{R}_{k+1} &= \mathbf{R}_k\mathbf{\Phi}_{\theta\theta} \\ \boldsymbol{\omega}_{k+1} &= \boldsymbol{\omega}_k \end{aligned} Rk+1ωk+1=RkΦθθ=ωk
其中 Φ θ θ \mathbf{\Phi}_{\theta\theta} Φθθ 是转移矩阵的旋转部分。
如果我们使用一阶泰勒近似,我们得到:
Φ θ θ ≈ I + [ ω ] × Δ t \mathbf{\Phi}_{\theta\theta} \approx \mathbf{I} + [\boldsymbol{\omega}]_\times \Delta t Φθθ≈I+[ω]×Δt
然而,使用封闭形式的解,我们得到:
Φ θ θ = R ( ω Δ t ) ⊤ = exp ( [ ω ] × Δ t ) \mathbf{\Phi}_{\theta\theta} = \mathbf{R}(\boldsymbol{\omega}\Delta t)^\top = \exp([\boldsymbol{\omega}]_\times \Delta t) Φθθ=R(ωΔt)⊤=exp([ω]×Δt)
这里, exp ( ⋅ ) \exp(\cdot) exp(⋅) 表示矩阵指数。
现在,让我们看看如何在Python中实现这一点:
import numpy as np
def skew(x):
return np.array([[0, -x[2], x[1]],
[x[2], 0, -x[0]],
[-x[1], x[0], 0]])
def exp_so3(x):
theta = np.linalg.norm(x)
if theta < 1e-8:
return np.eye(3)
else:
u = x / theta
return np.eye(3) + np.sin(theta) * skew(u) + (1 - np.cos(theta)) * np.dot(skew(u), skew(u))
# 状态预测
def predict_state(R, omega, dt):
R_pred = np.dot(R, exp_so3(omega * dt).T)
omega_pred = omega
return R_pred, omega_pred
这里,skew函数创建一个叉积矩阵,exp_so3函数计算SO(3)上的矩阵指数(使用罗德里格斯公式)。在predict_state函数中,我们使用封闭形式的解来预测旋转矩阵,使用简单的积分来预测角速度(因为我们假设角加速度为零)。
使用封闭形式的解的好处是,无论时间步长 Δ t \Delta t Δt 有多大,它总是给出一个有效的旋转矩阵(即,正交且行列式为1)。相比之下,使用泰勒近似,我们需要非常小的时间步长来保持数值稳定性,特别是对于大的角速度。
在实际应用中,我们通常会结合使用封闭形式的解和泰勒近似。对于状态中的旋转部分,我们使用封闭形式的解,而对于其他部分(如位置、速度等),我们使用泰勒近似。这提供了精度和效率之间的最佳平衡。
总之,使用旋转项的封闭形式解是状态估计中的一个重要技巧。它提高了数值稳定性和精度,特别是在处理大的旋转或长的时间步长时。通过在适当的地方使用封闭形式的解,我们可以设计出更鲁棒、更准确的状态估计算法。
7.1.2 N阶截断
截断到更高阶将提高近似转移矩阵的精度。一个特别有趣的截断阶数是最大程度地利用结果的稀疏性。换句话说,在没有新的非零项出现之后的阶数。
对于6.2节中的简化IMU示例,这个阶数是2,结果是
Φ
≈
I
+
A
Δ
t
+
1
2
A
2
Δ
t
2
=
[
I
I
Δ
t
−
1
2
R
[
a
]
×
Δ
t
2
0
I
−
R
[
a
]
×
(
I
−
1
2
[
ω
]
×
Δ
t
)
Δ
t
0
0
R
{
ω
Δ
t
}
⊤
]
\mathbf{\Phi} \approx \mathbf{I} + \mathbf{A}\Delta t + \frac{1}{2}\mathbf{A}^2\Delta t^2 = \\ \begin{bmatrix} \mathbf{I} & \mathbf{I}\Delta t & -\frac{1}{2}\mathbf{R}[\mathbf{a}]_\times \Delta t^2 \\ \mathbf{0} & \mathbf{I} & -\mathbf{R}[\mathbf{a}]_\times (\mathbf{I} - \frac{1}{2}[\boldsymbol{\omega}]_\times \Delta t)\Delta t \\ \mathbf{0} & \mathbf{0} & \mathbf{R}\{\boldsymbol{\omega}\Delta t\}^\top \end{bmatrix}
Φ≈I+AΔt+21A2Δt2=
I00IΔtI0−21R[a]×Δt2−R[a]×(I−21[ω]×Δt)ΔtR{ωΔt}⊤
在6.3节的完整IMU示例中,这个阶数是3,结果是
Φ ≈ I + A Δ t + 1 2 A 2 Δ t 2 + 1 6 A 3 Δ t 3 \mathbf{\Phi} \approx \mathbf{I} + \mathbf{A}\Delta t + \frac{1}{2}\mathbf{A}^2\Delta t^2 + \frac{1}{6}\mathbf{A}^3\Delta t^3 Φ≈I+AΔt+21A2Δt2+61A3Δt3
其完整形式由于空间原因在此不给出。读者可以参考6.3节中 A \mathbf{A} A、 A 2 \mathbf{A}^2 A2 和 A 3 \mathbf{A}^3 A3 的表达式。
7.2 分块截断
通过在转移矩阵的每个块的泰勒级数中截断到第一个重要项,可以得到与之前解释的封闭形式非常接近的近似。也就是说,我们不是按照 A \mathbf{A} A 的完整幂次截断级数,而是单独考虑每个块。因此,需要在每个块的基础上分析截断。我们用两个例子来探讨它。
对于6.2节中的简化IMU示例,我们有级数 Σ 1 \mathbf{\Sigma}_1 Σ1 和 Σ 2 \mathbf{\Sigma}_2 Σ2,我们可以如下截断它们
Σ 1 = I Δ t + 1 2 Θ θ Δ t 2 + ⋯ ≈ I Δ t Σ 2 = 1 2 I Δ t 2 + 1 3 ! Θ θ Δ t 3 + ⋯ ≈ 1 2 I Δ t 2 \begin{aligned} \mathbf{\Sigma}_1 &= \mathbf{I}\Delta t + \frac{1}{2}\mathbf{\Theta}_\theta \Delta t^2 + \cdots \approx \mathbf{I}\Delta t \\ \mathbf{\Sigma}_2 &= \frac{1}{2}\mathbf{I}\Delta t^2 + \frac{1}{3!}\mathbf{\Theta}_\theta \Delta t^3 + \cdots \approx \frac{1}{2}\mathbf{I}\Delta t^2 \end{aligned} Σ1Σ2=IΔt+21ΘθΔt2+⋯≈IΔt=21IΔt2+3!1ΘθΔt3+⋯≈21IΔt2
这导致近似的转移矩阵
Φ ≈ [ I I Δ t − 1 2 R [ a ] × Δ t 2 0 I − R [ a ] × Δ t 0 0 R ( ω Δ t ) ⊤ ] \mathbf{\Phi} \approx \begin{bmatrix} \mathbf{I} & \mathbf{I}\Delta t & -\frac{1}{2}\mathbf{R}[\mathbf{a}]_\times \Delta t^2 \\ \mathbf{0} & \mathbf{I} & -\mathbf{R}[\mathbf{a}]_\times \Delta t \\ \mathbf{0} & \mathbf{0} & \mathbf{R}(\boldsymbol{\omega}\Delta t)^\top \end{bmatrix} Φ≈ I00IΔtI0−21R[a]×Δt2−R[a]×ΔtR(ωΔt)⊤
它比上面系统范围内的一阶截断(因为现在出现了右上角项)更准确,但仍然容易获得和计算,特别是与B节中开发的封闭形式相比。再次观察到,我们为最低阶项取了封闭形式,即 Φ θ θ = R ( ω Δ t ) ⊤ \mathbf{\Phi}_{\theta\theta} = \mathbf{R}(\boldsymbol{\omega}\Delta t)^\top Φθθ=R(ωΔt)⊤。
在一般情况下,只需将每个 Σ n \mathbf{\Sigma}_n Σn (除了 Σ 0 \mathbf{\Sigma}_0 Σ0)近似为其级数的第一项,即
Σ 0 = R { ω Δ t } ⊤ , Σ n > 0 ≈ 1 n ! I Δ t n \mathbf{\Sigma}_0 = \mathbf{R}\{\boldsymbol{\omega}\Delta t\}^\top, \quad \mathbf{\Sigma}_{n>0} \approx \frac{1}{n!}\mathbf{I}\Delta t^n Σ0=R{ωΔt}⊤,Σn>0≈n!1IΔtn
对于完整的IMU示例,将上述 Σ n \mathbf{\Sigma}_n Σn 代入前面介绍的公式得到近似的转移矩阵,
Φ ≈ [ I I Δ t − 1 2 R [ a ] × Δ t 2 − 1 2 R Δ t 2 1 3 ! R [ a ] × Δ t 3 1 2 I Δ t 2 0 I − R [ a ] × Δ t − R Δ t 1 2 R [ a ] × Δ t 2 I Δ t 0 0 R { ω Δ t } ⊤ 0 − I Δ t 0 0 0 0 I 0 0 0 0 0 0 I 0 0 0 0 0 0 I ] \mathbf{\Phi} \approx \begin{bmatrix} \mathbf{I} & \mathbf{I}\Delta t & -\frac{1}{2}\mathbf{R}[\mathbf{a}]_\times \Delta t^2 & -\frac{1}{2}\mathbf{R}\Delta t^2 & \frac{1}{3!}\mathbf{R}[\mathbf{a}]_\times \Delta t^3 & \frac{1}{2}\mathbf{I}\Delta t^2 \\ \mathbf{0} & \mathbf{I} & -\mathbf{R}[\mathbf{a}]_\times \Delta t & -\mathbf{R}\Delta t & \frac{1}{2}\mathbf{R}[\mathbf{a}]_\times \Delta t^2 & \mathbf{I}\Delta t \\ \mathbf{0} & \mathbf{0} & \mathbf{R}\{\boldsymbol{\omega}\Delta t\}^\top & \mathbf{0} & -\mathbf{I}\Delta t & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} \end{bmatrix} Φ≈ I00000IΔtI0000−21R[a]×Δt2−R[a]×ΔtR{ωΔt}⊤000−21RΔt2−RΔt0I003!1R[a]×Δt321R[a]×Δt2−IΔt0I021IΔt2IΔt000I
其中
a = a m − a b , ω = ω m − ω b , R = R { q } \mathbf{a} = \mathbf{a}_m - \mathbf{a}_b, \quad \boldsymbol{\omega} = \boldsymbol{\omega}_m - \boldsymbol{\omega}_b, \quad \mathbf{R} = \mathbf{R}\{\mathbf{q}\} a=am−ab,ω=ωm−ωb,R=R{q}
这个方法的一个简单变体是将矩阵中的每个块限制在某个最大阶数 n n n。对于 n = 1 n=1 n=1,我们有
Φ ≈ [ I I Δ t 0 0 0 0 0 I − R [ a ] × Δ t − R Δ t 0 I Δ t 0 0 R { ω Δ t } ⊤ 0 − I Δ t 0 0 0 0 I 0 0 0 0 0 0 I 0 0 0 0 0 0 I ] \mathbf{\Phi} \approx \begin{bmatrix} \mathbf{I} & \mathbf{I}\Delta t & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{I} & -\mathbf{R}[\mathbf{a}]_\times \Delta t & -\mathbf{R}\Delta t & \mathbf{0} & \mathbf{I}\Delta t \\ \mathbf{0} & \mathbf{0} & \mathbf{R}\{\boldsymbol{\omega}\Delta t\}^\top & \mathbf{0} & -\mathbf{I}\Delta t & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} \end{bmatrix} Φ≈ I00000IΔtI00000−R[a]×ΔtR{ωΔt}⊤0000−RΔt0I0000−IΔt0I00IΔt000I
这是欧拉方法,而对于 n = 2 n=2 n=2,
Φ ≈ [ I I Δ t − 1 2 R [ a ] × Δ t 2 − 1 2 R Δ t 2 0 1 2 I Δ t 2 0 I − R [ a ] × Δ t − R Δ t 1 2 R [ a ] × Δ t 2 I Δ t 0 0 R { ω Δ t } ⊤ 0 − I Δ t 0 0 0 0 I 0 0 0 0 0 0 I 0 0 0 0 0 0 I ] \mathbf{\Phi} \approx \begin{bmatrix} \mathbf{I} & \mathbf{I}\Delta t & -\frac{1}{2}\mathbf{R}[\mathbf{a}]_\times \Delta t^2 & -\frac{1}{2}\mathbf{R}\Delta t^2 & \mathbf{0} & \frac{1}{2}\mathbf{I}\Delta t^2 \\ \mathbf{0} & \mathbf{I} & -\mathbf{R}[\mathbf{a}]_\times \Delta t & -\mathbf{R}\Delta t & \frac{1}{2}\mathbf{R}[\mathbf{a}]_\times \Delta t^2 & \mathbf{I}\Delta t \\ \mathbf{0} & \mathbf{0} & \mathbf{R}\{\boldsymbol{\omega}\Delta t\}^\top & \mathbf{0} & -\mathbf{I}\Delta t & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} \end{bmatrix} Φ≈ I00000IΔtI0000−21R[a]×Δt2−R[a]×ΔtR{ωΔt}⊤000−21RΔt2−RΔt0I00021R[a]×Δt2−IΔt0I021IΔt2IΔt000I
对于 n ≥ 3 n \geq 3 n≥3,我们有完整的其它形式，之后再介绍。
Python代码示例
以下是使用截断级数近似转移矩阵的Python代码示例。这个例子基于之前的IMU积分代码,并展示了如何使用不同的截断阶数。
import numpy as np
def skew(x):
return np.array([[0, -x[2], x[1]],
[x[2], 0, -x[0]],
[-x[1], x[0], 0]])
def exp_so3(x):
theta = np.linalg.norm(x)
if theta < 1e-8:
return np.eye(3)
else:
u = x / theta
return np.eye(3) + np.sin(theta) * skew(u) + (1 - np.cos(theta)) * np.dot(skew(u), skew(u))
def transition_matrix_trunc(F, dt, order):
phi = np.eye(F.shape[0])
term = np.eye(F.shape[0])
for i in range(1, order + 1):
term = np.dot(term, F * dt) / i
phi += term
return phi
# 假设我们有IMU测量值和状态估计
a_m = np.array([0.1, 0.2, 9.8])
w_m = np.array([0.01, -0.02, 0.03])
v_est = np.array([1.0, 2.0, 3.0])
q_est = np.array([0.99, 0.0, 0.1, 0.0])
dt = 0.01
# 计算状态转移矩阵
R = exp_so3(q_est[1:] * 2)
a_est = np.dot(R, a_m)
F = np.zeros((9, 9))
F[:3, 3:6] = np.eye(3)
F[3:6, 6:] = -skew(a_est)
# 使用不同的截断阶数
phi_1st = transition_matrix_trunc(F, dt, 1)
phi_2nd = transition_matrix_trunc(F, dt, 2)
print("First-order approximation:")
print(phi_1st)
print("Second-order approximation:")
print(phi_2nd)
这个例子首先定义了一些辅助函数,如 skew() 用于创建叉积矩阵,exp_so3() 用于计算SO(3)上的指数映射。主函数 transition_matrix_trunc() 使用截断泰勒级数来近似转移矩阵,截断阶数由 order 参数控制。
在主程序中,我们假设有一些IMU测量值和状态估计。我们使用这些值构造系统雅可比矩阵
F
\mathbf{F}
F,然后使用不同的截断阶数(在本例中为1和2)调用 transition_matrix_trunc() 函数。
这个例子展示了如何使用截断泰勒级数来近似转移矩阵,以及截断阶数如何影响近似精度。在实际应用中,选择适当的截断阶数需要在计算复杂性和所需精度之间进行权衡。
应用案例
使用截断级数近似转移矩阵在实时状态估计中非常有用,在这种情况下,计算精确的转移矩阵可能计算量太大。例如,在他们的论文中,Eckenhoff等人[1]使用截断的泰勒级数来近似视觉惯性导航系统的转移矩阵。
在他们的工作中,状态向量包括位置、速度、方向(四元数)以及IMU偏差。系统雅可比矩阵是使用这些状态变量的函数构建的。然后,他们使用截断的泰勒级数来近似离散时间转移矩阵,类似于我们的示例。
通过控制泰勒级数的截断阶数,他们能够在计算复杂性和估计精度之间进行权衡。这允许他们的系统在资源有限的平台上实时运行,同时仍然保持良好的估计精度。
这个案例研究突出了截断级数近似在实际状态估计问题中的有用性。通过理解这种近似的原理及其与精确转移矩阵的关系,我们可以设计出计算效率高、精度高的状态估计算法。
7.3 通过龙格-库塔积分的转移矩阵
还有另一种近似转移矩阵的方法是使用龙格-库塔(Runge-Kutta, RK)积分。当动态矩阵 A \mathbf{A} A 不能认为在积分区间内是常数时,这可能是必要的,即
x ˙ ( t ) = A ( t ) x ( t ) \dot{\mathbf{x}}(t) = \mathbf{A}(t)\mathbf{x}(t) x˙(t)=A(t)x(t)
让我们重写以下两个关系,它们定义了相同的系统在连续时间和离散时间中的关系。它们涉及动态矩阵 A \mathbf{A} A 和转移矩阵 Φ \mathbf{\Phi} Φ,
x ˙ ( t ) = A ( t ) ⋅ x ( t ) x ( t n + τ ) = Φ ( t n + τ ∣ t n ) ⋅ x ( t n ) \begin{aligned} \dot{\mathbf{x}}(t) &= \mathbf{A}(t) \cdot \mathbf{x}(t) \\ \mathbf{x}(t_n + \tau) &= \mathbf{\Phi}(t_n + \tau | t_n) \cdot \mathbf{x}(t_n) \end{aligned} x˙(t)x(tn+τ)=A(t)⋅x(t)=Φ(tn+τ∣tn)⋅x(tn)
这些方程允许我们以两种方式发展 x ˙ ( t n + τ ) \dot{\mathbf{x}}(t_n + \tau) x˙(tn+τ) (左右两边的发展,请注意表示时间导数的小圆点),
( Φ ( t n + τ ∣ t n ) x ( t n ) ) ˙ = x ˙ ( t n + τ ) = A ( t n + τ ) x ( t n + τ ) Φ ˙ ( t n + τ ∣ t n ) x ( t n ) + Φ ( t n + τ ∣ t n ) x ˙ ( t n ) = A ( t n + τ ) Φ ( t n + τ ∣ t n ) x ( t n ) \begin{aligned} \dot{(\mathbf{\Phi}(t_n + \tau | t_n)\mathbf{x}(t_n))} &= \dot{\mathbf{x}}(t_n + \tau) = \mathbf{A}(t_n + \tau)\mathbf{x}(t_n + \tau) \\ \dot{\mathbf{\Phi}}(t_n + \tau | t_n)\mathbf{x}(t_n) + \mathbf{\Phi}(t_n + \tau | t_n)\dot{\mathbf{x}}(t_n) &= \mathbf{A}(t_n + \tau)\mathbf{\Phi}(t_n + \tau | t_n)\mathbf{x}(t_n) \end{aligned} (Φ(tn+τ∣tn)x(tn))˙Φ˙(tn+τ∣tn)x(tn)+Φ(tn+τ∣tn)x˙(tn)=x˙(tn+τ)=A(tn+τ)x(tn+τ)=A(tn+τ)Φ(tn+τ∣tn)x(tn)
这里,来自注意到 x ˙ ( t n ) = x ˙ n = 0 \dot{\mathbf{x}}(t_n) = \dot{\mathbf{x}}_n = 0 x˙(tn)=x˙n=0,因为它是一个采样值。然后,
Φ ˙ ( t n + τ ∣ t n ) = A ( t n + τ ) Φ ( t n + τ ∣ t n ) \dot{\mathbf{\Phi}}(t_n + \tau | t_n) = \mathbf{A}(t_n + \tau)\mathbf{\Phi}(t_n + \tau | t_n) Φ˙(tn+τ∣tn)=A(tn+τ)Φ(tn+τ∣tn)
相同的ODE,现在应用于转移矩阵而不是状态向量。注意,由于恒等式 x ( t n ) = Φ t n ∣ t n x ( t n ) \mathbf{x}(t_n) = \mathbf{\Phi}_{t_n|t_n}\mathbf{x}(t_n) x(tn)=Φtn∣tnx(tn),在区间开始时 t = t n t=t_n t=tn,转移矩阵总是单位矩阵,
Φ t n ∣ t n = I \mathbf{\Phi}_{t_n|t_n} = \mathbf{I} Φtn∣tn=I
使用RK4与 f ( t , Φ ( t ) ) = A ( t ) Φ ( t ) f(t, \mathbf{\Phi}(t)) = \mathbf{A}(t)\mathbf{\Phi}(t) f(t,Φ(t))=A(t)Φ(t),我们有
Φ ≜ Φ ( t n + Δ t ∣ t n ) = I + Δ t 6 ( K 1 + 2 K 2 + 2 K 3 + K 4 ) \mathbf{\Phi} \triangleq \mathbf{\Phi}(t_n + \Delta t | t_n) = \mathbf{I} + \frac{\Delta t}{6}(\mathbf{K}_1 + 2\mathbf{K}_2 + 2\mathbf{K}_3 + \mathbf{K}_4) Φ≜Φ(tn+Δt∣tn)=I+6Δt(K1+2K2+2K3+K4)
其中
K 1 = A ( t n ) K 2 = A ( t n + 1 2 Δ t ) ( I + Δ t 2 K 1 ) K 3 = A ( t n + 1 2 Δ t ) ( I + Δ t 2 K 2 ) K 4 = A ( t n + Δ t ) ( I + Δ t ⋅ K 3 ) \begin{aligned} \mathbf{K}_1 &= \mathbf{A}(t_n) \\ \mathbf{K}_2 &= \mathbf{A}(t_n + \frac{1}{2}\Delta t)(\mathbf{I} + \frac{\Delta t}{2}\mathbf{K}_1) \\ \mathbf{K}_3 &= \mathbf{A}(t_n + \frac{1}{2}\Delta t)(\mathbf{I} + \frac{\Delta t}{2}\mathbf{K}_2) \\ \mathbf{K}_4 &= \mathbf{A}(t_n + \Delta t)(\mathbf{I} + \Delta t \cdot \mathbf{K}_3) \end{aligned} K1K2K3K4=A(tn)=A(tn+21Δt)(I+2ΔtK1)=A(tn+21Δt)(I+2ΔtK2)=A(tn+Δt)(I+Δt⋅K3)
上面的公式描述了如何使用四阶龙格-库塔方法(RK4)来数值地计算状态转移矩阵 Φ ( t n + Δ t ∣ t n ) \mathbf{\Phi}(t_n + \Delta t | t_n) Φ(tn+Δt∣tn)。这在系统矩阵 A ( t ) \mathbf{A}(t) A(t) 随时间变化的情况下特别有用,因为在这种情况下,我们无法像在时不变系统中那样直接计算矩阵指数。
RK4方法通过在每个时间步内的多个点评估系统矩阵 A ( t ) \mathbf{A}(t) A(t) 来近似转移矩阵。这些评估点是 t n t_n tn, t n + 1 2 Δ t t_n + \frac{1}{2}\Delta t tn+21Δt, 和 t n + Δ t t_n + \Delta t tn+Δt。在每个评估点,我们计算一个 “斜率” K i \mathbf{K}_i Ki,这表示转移矩阵在该点的变化率。然后,这些斜率被加权平均以得到转移矩阵在整个时间步长上的平均变化率。
让我们以一个简单的时变系统为例。假设我们有一个二维状态向量 x = [ x 1 , x 2 ] ⊤ \mathbf{x} = [x_1, x_2]^\top x=[x1,x2]⊤,系统矩阵为:
A ( t ) = [ 0 1 − k ( t ) − c ] \mathbf{A}(t) = \begin{bmatrix} 0 & 1 \\ -k(t) & -c \end{bmatrix} A(t)=[0−k(t)1−c]
其中 k ( t ) k(t) k(t) 是一个时变参数, c c c 是一个常数。这可以表示一个随时间变化的弹簧常数的质量-弹簧-阻尼器系统。
系统的状态转移由以下微分方程给出:
Φ ˙ ( t ∣ t n ) = A ( t ) Φ ( t ∣ t n ) , Φ ( t n ∣ t n ) = I \dot{\mathbf{\Phi}}(t | t_n) = \mathbf{A}(t)\mathbf{\Phi}(t | t_n), \quad \mathbf{\Phi}(t_n | t_n) = \mathbf{I} Φ˙(t∣tn)=A(t)Φ(t∣tn),Φ(tn∣tn)=I
现在,让我们使用RK4方法在时间步长 Δ t \Delta t Δt 上数值积分这个方程:
import numpy as np
def A(t):
k = 1 + np.sin(t) # 一个示例时变参数
c = 0.1
return np.array([[0, 1], [-k, -c]])
def rk4_step(Phi, t, dt):
K1 = np.dot(A(t), Phi)
K2 = np.dot(A(t + 0.5 * dt), Phi + 0.5 * dt * K1)
K3 = np.dot(A(t + 0.5 * dt), Phi + 0.5 * dt * K2)
K4 = np.dot(A(t + dt), Phi + dt * K3)
return Phi + (dt / 6) * (K1 + 2 * K2 + 2 * K3 + K4)
# RK4 积分示例
t0 = 0
t1 = 10
dt = 0.01
t = np.arange(t0, t1, dt)
Phi = np.eye(2) # 初始条件: Phi(t0 | t0) = I
Phi_list = [Phi]
for i in range(len(t) - 1):
Phi = rk4_step(Phi, t[i], dt)
Phi_list.append(Phi)
# 绘制结果
import matplotlib.pyplot as plt
plt.figure(figsize=(8, 6))
plt.plot(t, [Phi[0, 0] for Phi in Phi_list], label=r'$\Phi_{11}$')
plt.plot(t, [Phi[0, 1] for Phi in Phi_list], label=r'$\Phi_{12}$')
plt.plot(t, [Phi[1, 0] for Phi in Phi_list], label=r'$\Phi_{21}$')
plt.plot(t, [Phi[1, 1] for Phi in Phi_list], label=r'$\Phi_{22}$')
plt.xlabel('Time')
plt.ylabel('State Transition Matrix Elements')
plt.legend()
plt.grid(True)
plt.show()
在这个例子中,A(t) 函数返回在时间
t
t
t 的系统矩阵。rk4_step 函数执行单个RK4步骤,使用上面给出的公式计算
K
1
,
K
2
,
K
3
,
K
4
\mathbf{K}_1, \mathbf{K}_2, \mathbf{K}_3, \mathbf{K}_4
K1,K2,K3,K4,然后返回更新后的转移矩阵。
在主程序中,我们设置初始时间
t
0
t_0
t0,最终时间
t
1
t_1
t1,和时间步长
Δ
t
\Delta t
Δt。然后,我们使用一个循环执行RK4积分,在每个时间步长上调用 rk4_step 函数。我们存储每个时间步长的转移矩阵,以便稍后绘图。
最后,我们使用 Matplotlib 绘制每个转移矩阵元素随时间的变化。这让我们可以可视化系统的状态转移行为随时间的演变。
RK4方法在处理状态估计问题的转移矩阵计算中非常有用,在这些问题中,系统动力学依赖于时间或状态本身。通过在每个时间步长内的多个点评估系统矩阵,RK4方法提供了一种在这些情况下准确地传播状态不确定性的方法。
总之,RK4积分是状态估计工具箱的一个重要工具。它允许我们处理时变系统,这在许多实际应用中是常见的。通过理解RK4方法的原理及其在状态转移矩阵计算中的应用,我们可以设计出更准确、更鲁棒的状态估计算法。
7.4 误差状态示例
让我们考虑非线性、时变系统的误差状态卡尔曼滤波器
x ˙ t ( t ) = f ( t , x t ( t ) , u ( t ) ) \dot{\mathbf{x}}_t(t) = \mathbf{f}(t, \mathbf{x}_t(t), \mathbf{u}(t)) x˙t(t)=f(t,xt(t),u(t))
其中 x t \mathbf{x}_t xt 表示真实状态, u \mathbf{u} u 是控制输入。这个真实状态是标称状态 x \mathbf{x} x 和误差状态 δ x \delta\mathbf{x} δx 的组合,用 ⊕ \oplus ⊕ 表示,
x t ( t ) = x ( t ) ⊕ δ x ( t ) \mathbf{x}_t(t) = \mathbf{x}(t) \oplus \delta\mathbf{x}(t) xt(t)=x(t)⊕δx(t)
其中误差状态动力学允许一个线性形式,它是时变的,取决于标称状态 x \mathbf{x} x 和控制 u \mathbf{u} u,即
δ x ˙ = A ( x ( t ) , u ( t ) ) ⋅ δ x \dot{\delta\mathbf{x}} = \mathbf{A}(\mathbf{x}(t), \mathbf{u}(t)) \cdot \delta\mathbf{x} δx˙=A(x(t),u(t))⋅δx
也就是说,误差状态动态矩阵的形式为 A ( t ) = A ( x ( t ) , u ( t ) ) \mathbf{A}(t) = \mathbf{A}(\mathbf{x}(t), \mathbf{u}(t)) A(t)=A(x(t),u(t))。误差状态转移矩阵的动力学可以写成
Φ ˙ ( t n + τ ∣ t n ) = A ( x ( t ) , u ( t ) ) ⋅ Φ ( t n + τ ∣ t n ) \dot{\mathbf{\Phi}}(t_n + \tau | t_n) = \mathbf{A}(\mathbf{x}(t), \mathbf{u}(t)) \cdot \mathbf{\Phi}(t_n + \tau | t_n) Φ˙(tn+τ∣tn)=A(x(t),u(t))⋅Φ(tn+τ∣tn)
为了用RK积分这个方程,我们需要在RK评估点处的 x ( t ) \mathbf{x}(t) x(t) 和 u ( t ) \mathbf{u}(t) u(t) 的值,对于RK4,这些点是 { t n , t n + Δ t / 2 , t n + Δ t } \{t_n, t_n + \Delta t/2, t_n + \Delta t\} {tn,tn+Δt/2,tn+Δt}。从简单的开始,评估点处的控制输入 u ( t ) \mathbf{u}(t) u(t) 可以通过当前和最后测量值的线性插值获得,
u ( t n ) = u n u ( t n + Δ t / 2 ) = u n + u n + 1 2 u ( t n + Δ t ) = u n + 1 \begin{aligned} \mathbf{u}(t_n) &= \mathbf{u}_n \\ \mathbf{u}(t_n + \Delta t/2) &= \frac{\mathbf{u}_n + \mathbf{u}_{n+1}}{2} \\ \mathbf{u}(t_n + \Delta t) &= \mathbf{u}_{n+1} \end{aligned} u(tn)u(tn+Δt/2)u(tn+Δt)=un=2un+un+1=un+1
标称状态动力学应该使用可实现的最佳积分来积分。例如,使用RK4积分,我们有
k 1 = f ( x n , u n ) k 2 = f ( x n + Δ t 2 k 1 , u n + u n + 1 2 ) k 3 = f ( x n + Δ t 2 k 2 , u n + u n + 1 2 ) k 4 = f ( x n + Δ t k 3 , u n + 1 ) k = ( k 1 + 2 k 2 + 2 k 3 + k 4 ) / 6 \begin{aligned} \mathbf{k}_1 &= \mathbf{f}(\mathbf{x}_n, \mathbf{u}_n) \\ \mathbf{k}_2 &= \mathbf{f}(\mathbf{x}_n + \frac{\Delta t}{2}\mathbf{k}_1, \frac{\mathbf{u}_n + \mathbf{u}_{n+1}}{2}) \\ \mathbf{k}_3 &= \mathbf{f}(\mathbf{x}_n + \frac{\Delta t}{2}\mathbf{k}_2, \frac{\mathbf{u}_n + \mathbf{u}_{n+1}}{2}) \\ \mathbf{k}_4 &= \mathbf{f}(\mathbf{x}_n + \Delta t\mathbf{k}_3, \mathbf{u}_{n+1}) \\ \mathbf{k} &= (\mathbf{k}_1 + 2\mathbf{k}_2 + 2\mathbf{k}_3 + \mathbf{k}_4)/6 \end{aligned} k1k2k3k4k=f(xn,un)=f(xn+2Δtk1,2un+un+1)=f(xn+2Δtk2,2un+un+1)=f(xn+Δtk3,un+1)=(k1+2k2+2k3+k4)/6
这给出了评估点处的估计值,
x ( t n ) = x n x ( t n + Δ t / 2 ) = x n + Δ t 2 k x ( t n + Δ t ) = x n + Δ t k \begin{aligned} \mathbf{x}(t_n) &= \mathbf{x}_n \\ \mathbf{x}(t_n + \Delta t/2) &= \mathbf{x}_n + \frac{\Delta t}{2}\mathbf{k} \\ \mathbf{x}(t_n + \Delta t) &= \mathbf{x}_n + \Delta t \mathbf{k} \end{aligned} x(tn)x(tn+Δt/2)x(tn+Δt)=xn=xn+2Δtk=xn+Δtk
我们在这里注意到 x ( t n + Δ t / 2 ) = x n + x n + 1 2 \mathbf{x}(t_n + \Delta t/2) = \frac{\mathbf{x}_n + \mathbf{x}_{n+1}}{2} x(tn+Δt/2)=2xn+xn+1,与我们用于控制的线性插值相同。鉴于RK更新的线性特性,这不应令人惊讶。
无论我们以何种方式获得标称状态值,我们现在都可以计算用于转移矩阵积分的RK4矩阵,
K 1 = A ( x n , u n ) K 2 = A ( x n + Δ t 2 k , u n + u n + 1 2 ) ( I + Δ t 2 K 1 ) K 3 = A ( x n + Δ t 2 k , u n + u n + 1 2 ) ( I + Δ t 2 K 2 ) K 4 = A ( x n + Δ t k , u n + 1 ) ( I + Δ t K 3 ) K = ( K 1 + 2 K 2 + 2 K 3 + K 4 ) / 6 \begin{aligned} \mathbf{K}_1 &= \mathbf{A}(\mathbf{x}_n, \mathbf{u}_n) \\ \mathbf{K}_2 &= \mathbf{A}(\mathbf{x}_n + \frac{\Delta t}{2}\mathbf{k}, \frac{\mathbf{u}_n + \mathbf{u}_{n+1}}{2})(\mathbf{I} + \frac{\Delta t}{2}\mathbf{K}_1) \\ \mathbf{K}_3 &= \mathbf{A}(\mathbf{x}_n + \frac{\Delta t}{2}\mathbf{k}, \frac{\mathbf{u}_n + \mathbf{u}_{n+1}}{2})(\mathbf{I} + \frac{\Delta t}{2}\mathbf{K}_2) \\ \mathbf{K}_4 &= \mathbf{A}(\mathbf{x}_n + \Delta t\mathbf{k}, \mathbf{u}_{n+1})(\mathbf{I} + \Delta t\mathbf{K}_3) \\ \mathbf{K} &= (\mathbf{K}_1 + 2\mathbf{K}_2 + 2\mathbf{K}_3 + \mathbf{K}_4)/6 \end{aligned} K1K2K3K4K=A(xn,un)=A(xn+2Δtk,2un+un+1)(I+2ΔtK1)=A(xn+2Δtk,2un+un+1)(I+2ΔtK2)=A(xn+Δtk,un+1)(I+ΔtK3)=(K1+2K2+2K3+K4)/6
最后得到,
Φ ≜ Φ t n + Δ t ∣ t n = I + Δ t K \mathbf{\Phi} \triangleq \mathbf{\Phi}_{t_n+\Delta t|t_n} = \mathbf{I} + \Delta t \mathbf{K} Φ≜Φtn+Δt∣tn=I+ΔtK
Python代码示例
以下是使用龙格-库塔方法计算转移矩阵的Python代码示例。这个例子假设我们有一个简单的二维系统,其中状态向量包括位置和速度,控制输入是加速度。
import numpy as np
def f(x, u):
return np.array([x[1], u])
def A(x, u):
return np.array([[0, 1], [0, 0]])
def rk4_transition_matrix(x, u, dt):
k1 = f(x, u)
K1 = A(x, u)
k2 = f(x + 0.5 * dt * k1, 0.5 * (u + u))
K2 = A(x + 0.5 * dt * k1, 0.5 * (u + u))
k3 = f(x + 0.5 * dt * k2, 0.5 * (u + u))
K3 = A(x + 0.5 * dt * k2, 0.5 * (u + u))
k4 = f(x + dt * k3, u)
K4 = A(x + dt * k3, u)
x_next = x + (dt / 6) * (k1 + 2 * k2 + 2 * k3 + k4)
Phi = np.eye(2) + (dt / 6) * (K1 + 2 * K2 + 2 * K3 + K4)
return x_next, Phi
# 示例用法
x = np.array([0, 0])
u = 1
dt = 0.1
for i in range(10):
x, Phi = rk4_transition_matrix(x, u, dt)
print(f"Step {i+1}:")
print(f"State: {x}")
print(f"Transition Matrix:\n{Phi}\n")
在这个例子中,f函数表示系统动力学,A函数表示系统雅可比矩阵。rk4_transition_matrix函数使用RK4方法计算下一个状态和转移矩阵。
在主程序中,我们初始化状态为[0, 0],控制输入为常数加速度1。然后我们迭代10步,每一步调用rk4_transition_matrix函数来更新状态和计算转移矩阵。
为什么我们需要这些技术?
截断级数近似和龙格-库塔积分在状态估计中非常有用,特别是当我们处理非线性系统或时变系统时。
-
计算效率:精确计算转移矩阵,特别是对于非线性系统,可能非常耗时。使用截断级数近似或数值积分方法如RK4,我们可以以较低的计算成本获得转移矩阵的良好近似。这在实时系统中尤其重要,在实时系统中,我们需要快速处理传入的测量值。
-
处理非线性:许多实际系统是非线性的,这意味着状态转移不能用简单的矩阵乘法来表示。通过在每个时间步线性化系统动力学(就像在扩展卡尔曼滤波器中所做的那样),我们可以使用这些技术来近似非线性系统在短时间内的行为。
-
时变系统:一些系统的动力学随时间变化,这意味着转移矩阵在每个时间步都不同。使用数值积分方法如RK4,我们可以在每个时间步重新计算转移矩阵,从而考虑系统动力学的变化。
-
精度控制:截断级数近似让我们可以控制近似的精度。通过包括更多的泰勒级数项,我们可以提高精度,代价是计算复杂度增加。这让我们可以根据具体应用在精度和效率之间进行权衡。
总之,截断级数近似和龙格-库塔积分是状态估计工具箱中的重要工具。它们让我们可以处理实际系统中常见的非线性和时变行为,同时保持计算效率。通过理解这些技术的原理及其局限性,我们可以设计出精确而高效的状态估计算法。
应用案例
这些技术在各种机器人状态估计问题中都有应用。例如,在他们的论文中,Andrle和Crassidis[2]使用RK4积分来传播航天器的姿态状态。他们的状态向量包括四元数和角速度,系统动力学是非线性的,包括陀螺仪偏差和外部扰动力矩。
在每个时间步,他们使用RK4方法数值积分状态向量。这涉及在每个RK4步骤评估系统动力学和雅可比矩阵,类似于我们的示例。然后,他们使用更新后的状态和转移矩阵在卡尔曼滤波器框架中进行测量更新。
通过使用RK4积分,他们能够准确地传播航天器的姿态状态,同时考虑到系统动力学的非线性。这导致了比使用简单的欧拉积分更准确的状态估计。
这个案例研究突出了龙格-库塔方法在处理非线性系统时的有用性。通过在每个时间步数值积分状态和转移矩阵,我们可以获得系统行为的更准确表示,从而提高整体状态估计精度。
总结
在本章中,我们探讨了使用截断级数近似和龙格-库塔积分来近似状态转移矩阵。我们讨论了系统级截断和块级截断的概念,以及如何使用RK4方法数值积分状态和转移矩阵。
这些技术在处理非线性和时变系统时特别有用,在这种情况下,精确的转移矩阵可能难以计算或计算成本高昂。通过使用截断级数近似或数值积分,我们可以获得转移矩阵的高效近似,从而能够实时执行状态估计。
我们还讨论了这些技术在机器人应用中的相关性,如视觉惯性导航和航天器姿态估计。通过理解这些技术的原理及其在实际系统中的应用,我们可以设计出鲁棒、高效的状态估计算法。
总的来说,截断级数近似和龙格-库塔积分是状态估计领域的基本工具。无论您是在学术界还是工业界工作,掌握这些技术都是设计和实现先进状态估计系统的关键。
参考文献
[1] Eckenhoff, Kevin, Patrick Geneva, and Guoquan Huang. “Closed-form preintegration methods for graph-based visual–inertial navigation.” The International Journal of Robotics Research 38.5 (2019): 563-586.
[2] Andrle, Michael S., and John L. Crassidis. “Geometric integration of quaternions.” Journal of Guidance, Control, and Dynamics 36.6 (2013): 1762-1767.
标签：Unleashing,mathbf,Python,Chapter7,矩阵,Phi,tn,Delta,frac From： https://blog.csdn.net/jiayoushijie/article/details/139307459