html5制作网站谁的好,亚马逊雨林现状,wordpress侧面小工具,特克斯与凯科斯群岛域名官方网站文章目录0. 前言1. 离散时间的IMU运动学方程2. 状态变量定义3. 补充公式4. IMU误差状态空间方程推导4.1. 旋转误差 δr^i1\delta\hat{\mathbf{r}}_{i1}δr^i14.2. 速度误差 δv^i1\delta\hat{\mathbf{v}}_{i1}δv^i14.3. 平移误差 δpi1\delta \mathbf{p}_{i1}δpi14.4. …
文章目录0. 前言1. 离散时间的IMU运动学方程2. 状态变量定义3. 补充公式4. IMU误差状态空间方程推导4.1. 旋转误差 δr^i1\delta\hat{\mathbf{r}}_{i1}δr^i14.2. 速度误差 δv^i1\delta\hat{\mathbf{v}}_{i1}δv^i14.3. 平移误差 δpi1\delta \mathbf{p}_{i1}δpi14.4. 角速度零偏误差 δb^wi1\delta \hat{b}_{w_{i1}}δb^wi14.5. 加速度零偏误差 δb^ai1\delta \hat{b}_{a_{i1}}δb^ai14.6. 整理成矩阵状态空间方程的形式0. 前言
本文推导的是 IMU 积分的误差状态空间方程预积分的推导要比积分复杂但是推到方法和本文是一样的因此这里只给出积分的推导方式。
另外本文的推导方式参考了 R2Live Supplementary Material 中给出的推导这种方法是基于李群李代数使用 全增量 的方法计算误差状态空间方程非常好理解。
1. 离散时间的IMU运动学方程
{pi1piviΔt12[Ri(ami−bai−nai)−g]Δt2vi1vi[Ri(ami−bai−nai)−g]ΔtRi1Riexp[(ωmi−bωi−nωi)Δt]bωi1bωinbωiΔtbai1bainbaiΔt\left\{\begin{array}{l} \mathbf{p}_{i1}\mathbf{p}_i\mathbf{v}_i \Delta t\frac{1}{2}\left[\mathbf{R}_i\left(\mathbf{a}_{m i}-\mathbf{b}_{a_i}-\mathbf{n}_{a i}\right)-\mathbf{g}\right] \Delta t^2 \\ \mathbf{v}_{i1}\mathbf{v}_i\left[\mathbf{R}_i\left(\mathbf{a}_{m i}-\mathbf{b}_{a i}-\mathbf{n}_{a i}\right)-\mathbf{g}\right] \Delta t \\ \mathbf{R}_{i1}\mathbf{R}_i \exp \left[\left(\mathbf{\omega}_{mi}-\mathbf{b}_{\omega i}-\mathbf{n}_{\omega i}\right) \Delta t\right] \\ \mathbf{b}_{\omega i1}\mathbf{b}_{\omega i}\mathbf{n}_{b_\omega i} \Delta t \\ \mathbf{b}_{a_i1}\mathbf{b}_{a i}\mathbf{n}_{b_a i} \Delta t \end{array}\right. ⎩⎨⎧pi1piviΔt21[Ri(ami−bai−nai)−g]Δt2vi1vi[Ri(ami−bai−nai)−g]ΔtRi1Riexp[(ωmi−bωi−nωi)Δt]bωi1bωinbωiΔtbai1bainbaiΔt
式中忽略了参考坐标系一般可以选择 worldworldworld 系 W{W}W此时 g[0,0,9.8]Tg[0, 0, 9.8]^Tg[0,0,9.8]Tnai,nωi\mathbf{n}_{a i},\mathbf{n}_{\omega i}nai,nωi 为IMU读数的高斯白噪声nbai,nbωi\mathbf{n}_{b_a i},\mathbf{n}_{b_\omega i}nbai,nbωi 为IMU零偏的随机游走的高斯分布。
2. 状态变量定义
名义状态 x^\hat{\mathbf{x}}x^估计出来的状态真正能算出来的状态真实状态 x\mathbf{x}x无法计算出来的状态永远不知道的状态只能用公式表示出来误差状态 δx\mathbf{\delta x}δx真实状态 x\mathbf{x}x 与 名义状态 x^\hat{\mathbf{x}}x^ 的差值关系式为xx^δx\mathbf{x} \hat{\mathbf{x}} \mathbf{\delta x}xx^δx即 真实状态 名义状态 误差状态。
比如 角速度 ω\mathbf{\omega}ω 的名义状态和真实状态 {名义值ωi^ωmi−b^ωi真实值ωiωi^−δb^ωi−nωiωmi−b^ωi−δb^ωi−nωi\left\{\begin{array}{l} 名义值\hat{\mathbf{\omega}_i} \mathbf{\omega}_{m i} - \hat{\mathbf{b}}_{\omega i} \\ 真实值\mathbf{\omega}_i \hat{\mathbf{\omega}_i} - \delta\hat{\mathbf{b}}_{\omega i} - \mathbf{n}_{\omega i} \mathbf{\omega}_{m i} - \hat{\mathbf{b}}_{\omega i} - \delta\hat{\mathbf{b}}_{\omega i} - \mathbf{n}_{\omega i} \end{array}\right. {名义值ωi^ωmi−b^ωi真实值ωiωi^−δb^ωi−nωiωmi−b^ωi−δb^ωi−nωi 加速度 a\mathbf{a}a 的名义状态和真实状态 {名义值ai^ami−b^ai真实值aiai^−δb^ai−naiami−b^ai−δb^ai−nai\left\{\begin{array}{l} 名义值\hat{\mathbf{a}_i} \mathbf{a}_{m i} - \hat{\mathbf{b}}_{a i} \\ 真实值\mathbf{a}_i \hat{\mathbf{a}_i} - \delta\hat{\mathbf{b}}_{a i} - \mathbf{n}_{a i} \mathbf{a}_{m i} - \hat{\mathbf{b}}_{a i} - \delta\hat{\mathbf{b}}_{a i} - \mathbf{n}_{a i} \end{array}\right. {名义值ai^ami−b^ai真实值aiai^−δb^ai−naiami−b^ai−δb^ai−nai
3. 补充公式 李代数上的增量和李群上的扰动之间的关系 exp(ϕΔϕ)exp(JlΔϕ)⋅exp(ϕ)exp(ϕ)⋅exp(JrΔϕ)\begin{align} \exp (\phi\Delta \phi) \exp (J_l \Delta \phi) \cdot \exp (\phi) \exp (\phi) \cdot \exp (J_r \Delta \phi)\end{align} exp(ϕΔϕ)exp(JlΔϕ)⋅exp(ϕ)exp(ϕ)⋅exp(JrΔϕ) SO(3)SO(3)SO(3)的伴随性质 R⊤exp(ϕ)Rexp(R⊤ϕ)\begin{align}R^{\top} \exp (\phi) R\exp \left(R^{\top} \phi\right)\end{align} R⊤exp(ϕ)Rexp(R⊤ϕ) BCH近似公式 log[exp(ϕ1)exp(ϕ2)]{Jl(ϕ2)−1ϕ1ϕ2,ϕ1≈0Jr(ϕ1)−1ϕ2ϕ1,ϕ2≈0\begin{align} \log \left[\exp \left(\phi_1\right) \exp \left(\phi_2\right)\right]\left\{\begin{array}{l}\operatorname{J_l}\left(\phi_2\right)^{-1} \phi_1\phi_2, \quad \phi_1 \approx 0 \\ \operatorname{J_r}\left(\phi_1\right)^{-1} \phi_2\phi_1, \quad \phi_2 \approx 0 \end{array}\right.\end{align} log[exp(ϕ1)exp(ϕ2)]{Jl(ϕ2)−1ϕ1ϕ2,ϕ1≈0Jr(ϕ1)−1ϕ2ϕ1,ϕ2≈0
4. IMU误差状态空间方程推导
4.1. 旋转误差 δr^i1\delta\hat{\mathbf{r}}_{i1}δr^i1
定义δr^i1log[R^i1⊤Ritt]名义值R^i1R^iexp(w^iΔt)真实值Ri1R^iexp(δr^i)⋅exp(ωiΔt)\begin{aligned} \text { 定义} \delta \hat{r}_{i1} \log \left[\hat{R}_{i1}^{\top} R_{i t t}\right] \\ \text {名义值} \hat{R}_{i1} \hat{R}_i \exp \left(\hat{w}_i \Delta t\right) \\ \text {真实值} R_{i1} \hat{R}_i \exp \left(\delta \hat{r}_i\right) \cdot \exp \left(\omega_i \Delta t\right) \end{aligned} 定义δr^i1名义值R^i1真实值Ri1log[R^i1⊤Ritt]R^iexp(w^iΔt)R^iexp(δr^i)⋅exp(ωiΔt)
则旋转误差如下
4.2. 速度误差 δv^i1\delta\hat{\mathbf{v}}_{i1}δv^i1
名义值v^i1v^i(R^ia^i−g)Δt真实值vi1(v^iδv^i)[R^iexp(δr^i)(a^i−δb^ai−nai)−g]Δt\begin{aligned} 名义值 \hat{v}_{i1}\hat{v}_i\left(\hat{R}_i \hat{a}_i-g\right) \Delta t \\ 真实值v_{i1} \left(\hat{v}_i\delta \hat{v}_i \right) \left[\hat{R}_i \exp \left(\delta \hat{r}_i\right)\left(\hat{a}_i-\delta \hat{b}_{a i}-n_{a i}\right)-g\right] \Delta t \end{aligned} 名义值v^i1真实值vi1v^i(R^ia^i−g)Δt(v^iδv^i)[R^iexp(δr^i)(a^i−δb^ai−nai)−g]Δt 4.3. 平移误差 δpi1\delta \mathbf{p}_{i1}δpi1
名义值 p^i1pi^v^iΔt12(Ri^ai^−g)Δt2真实值 pi1(p^iδp^i)(v^iδv^i)Δt12[R^i⋅exp(δr^i)(a^i−δb^ai−nai)−g]⋅Δt2\begin{aligned} \text { 名义值 } \hat{p}_{i1}\hat{p_i}\hat{v}_i \Delta t\frac{1}{2}\left(\hat{R_i} \hat{a_i}-g\right) \Delta t^2 \\ \text { 真实值 } p_{i1}\left(\hat{p}_i\delta \hat{p}_i\right)\left(\hat{v}_i\delta \hat{v}_i\right) \Delta t\frac{1}{2}\left[\hat{R}_i \cdot \exp \left(\delta \hat{r}_i\right)\left(\hat{a}_i-\delta \hat{b}_{ai}-n_{ai}\right) -g\right] \cdot \Delta t^2 \\ \end{aligned} 名义值 p^i1pi^v^iΔt21(Ri^ai^−g)Δt2 真实值 pi1(p^iδp^i)(v^iδv^i)Δt21[R^i⋅exp(δr^i)(a^i−δb^ai−nai)−g]⋅Δt2 4.4. 角速度零偏误差 δb^wi1\delta \hat{b}_{w_{i1}}δb^wi1
名义值b^wi1b^wi真实值bwi1b^wiδbwiΔt⋅nbwi误差δbwi1δbwiΔt⋅nbwi\begin{aligned} \text { 名义值} \hat{b}_{w_{ i1}} \hat{b}_{w i} \\ \text { 真实值} b_{w_{i1} } \hat{b}_{w_i}\delta b_{w i}\Delta t \cdot n_{b_{wi}} \\ \text { 误差} \delta b_{w_{i1} } \delta b_{w i}\Delta t \cdot n_{b_{wi}} \\ \end{aligned} 名义值b^wi1 真实值bwi1 误差δbwi1b^wib^wiδbwiΔt⋅nbwiδbwiΔt⋅nbwi
4.5. 加速度零偏误差 δb^ai1\delta \hat{b}_{a_{i1}}δb^ai1
名义值b^ai1b^ai真实值bai1b^aiδbaiΔt⋅nbai误差δbai1δbaiΔt⋅nbai\begin{aligned} \text { 名义值} \hat{b}_{a_{ i1}} \hat{b}_{a i} \\ \text { 真实值} b_{a_{i1} } \hat{b}_{a_i}\delta b_{a i}\Delta t \cdot n_{b_{ai}} \\ \text { 误差} \delta b_{a_{i1} } \delta b_{a i}\Delta t \cdot n_{b_{ai}} \\ \end{aligned} 名义值b^ai1 真实值bai1 误差δbai1b^aib^aiδbaiΔt⋅nbaiδbaiΔt⋅nbai
4.6. 整理成矩阵状态空间方程的形式
定义IMU运动学的矩阵状态空间方程形式为
δx^i1Fx⋅δx^iFw⋅wi\delta \hat{\mathbf{x}}_{i1}\mathbf{F}_x \cdot \delta \hat{\mathbf{x}}_i \mathbf{F}_w \cdot \mathbf{w}_i δx^i1Fx⋅δx^iFw⋅wi
其中状态变量的定义为
δx^i[δr^i,δp^i,δv^i,δb^wi,δb^ai]⊤wi[δnwi,δnai,δnbwi,δnbai]⊤\begin{aligned} \delta \hat{\mathbf{x}}_i \left[\delta \hat{\mathbf{r}}_i, ~~\delta \hat{\mathbf{p}}_i, ~~\delta \hat{\mathbf{v}}_i, ~~\delta \hat{\mathbf{b}}_{wi}, ~~\delta \hat{\mathbf{b}}_{ai}\right]^{\top} \\ \mathbf{w}_i \left[\delta\mathbf{n}_{wi}, ~~\delta\mathbf{n}_{ai}, ~~\delta\mathbf{n}_{b_{wi}}, ~~ \delta\mathbf{n}_{b_{ai}}\right]^{\top} \end{aligned} δx^iwi[δr^i, δp^i, δv^i, δb^wi, δb^ai]⊤[δnwi, δnai, δnbwi, δnbai]⊤
则系数矩阵为
Fx[exp(−ωiΔt^)00−IΔt00IIΔt00−R^i(a^i)×Δt0I0−R^iΔt000I000000]Fω[−IΔt00000000−R^iΔt0000IΔt0000IΔt]\begin{aligned} \mathbf{F}_x \left[\begin{array}{ccccc} \exp \left(-\hat{\omega_i \Delta t}\right) 0 0 -I \Delta t 0 \\ 0 I I \Delta t 0 0\\ -\hat{R}_i\left(\hat{a}_i\right)_{\times} \Delta t 0 I 0 -\hat{R}_i \Delta t \\ 0 0 0 I 0 \\ 0 0 0 0 0 \end{array}\right] \\\\ F_\omega \left[\begin{array}{cccc} -I \Delta t 0 0 0 \\ 0 0 0 0 \\ 0 -\hat{R}_i \Delta t 0 0 \\ 0 0 I \Delta t 0 \\ 0 0 0 I \Delta t \end{array}\right] \end{aligned} FxFωexp(−ωiΔt^)0−R^i(a^i)×Δt000I0000IΔtI00−IΔt00I000−R^iΔt00−IΔt000000−R^iΔt00000IΔt00000IΔt
协方差矩阵的传播公式为
Pi1FxPiFx⊤FωQFω⊤\mathbf{P}_{i1}\mathbf{F}_x \mathbf{P}_i \mathbf{F}_x{ }^{\top}\mathbf{F}_\omega \mathbf{Q} \mathbf{F}_\omega{ }^{\top} Pi1FxPiFx⊤FωQFω⊤
其中 Q\mathbf{Q}Q 为测量噪声的协方差矩阵。
