[MSCKF-VIO]状态传播：四阶龙格库塔积分

状态传播：四阶龙格库塔积分

文章简介

本文是MSCKF VIO系列文章的第三篇，详细介绍状态传播的实现过程。状态传播是扩展卡尔曼滤波的预测步骤，使用IMU测量值积分来预测状态的演变。

状态传播：四阶龙格库塔积分
- 文章简介
- 目录
- [1. 状态传播概述](#1. 状态传播概述)
- - [1.1 预测步骤的作用](#1.1 预测步骤的作用)
  - [1.2 连续时间系统模型](#1.2 连续时间系统模型)
- [2. 数值积分方法](#2. 数值积分方法)
- - [2.1 欧拉方法（一阶）](#2.1 欧拉方法（一阶）)
  - [2.2 中点方法（二阶）](#2.2 中点方法（二阶）)
  - [2.3 龙格库塔方法（四阶）](#2.3 龙格库塔方法（四阶）)
- [3. 代码实现](#3. 代码实现)
- - [3.1 批量IMU处理](#3.1 批量IMU处理)
  - [3.2 过程模型](#3.2 过程模型)
  - [3.3 龙格库塔积分实现](#3.3 龙格库塔积分实现)
- [4. 数学推导](#4. 数学推导)
- - [4.1 四元数微分方程求解](#4.1 四元数微分方程求解)
  - [4.2 速度和位置积分](#4.2 速度和位置积分)
- [5. 系统矩阵详解](#5. 系统矩阵详解)
- - [5.1 F矩阵（系统矩阵）](#5.1 F矩阵（系统矩阵）)
  - [5.2 G矩阵（噪声输入矩阵）](#5.2 G矩阵（噪声输入矩阵）)
- [6. 实现细节](#6. 实现细节)
- - [6.1 时间同步](#6.1 时间同步)
  - [6.2 偏置去除](#6.2 偏置去除)
  - [6.3 四元数归一化](#6.3 四元数归一化)
  - [6.4 数值稳定性](#6.4 数值稳定性)
- [7. 性能优化](#7. 性能优化)
- - [7.1 计算复杂度](#7.1 计算复杂度)
  - [7.2 内存使用](#7.2 内存使用)
- [8. 调试技巧](#8. 调试技巧)
- - [8.1 验证积分精度](#8.1 验证积分精度)
  - [8.2 状态变化监控](#8.2 状态变化监控)
- [9. 常见问题](#9. 常见问题)
- - [9.1 积分发散](#9.1 积分发散)
  - [9.2 姿态漂移](#9.2 姿态漂移)
- [10. 总结](#10. 总结)
- 下一篇预告
- 参考文献

1. 状态传播概述

1.1 预测步骤的作用

在扩展卡尔曼滤波中，预测步骤的作用：

状态预测：使用系统模型预测下一时刻的状态
不确定性传播：预测状态不确定性的演变
时间对齐：将状态传播到测量时刻

1.2 连续时间系统模型

IMU状态的连续时间微分方程：

姿态：

q ˙ W I = 1 2 Ω ( ω m − b g ) q W I \dot{\mathbf{q}}_{WI} = \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}_m - \mathbf{b}g) \mathbf{q}{WI} q˙WI=21Ω(ωm−bg)qWI

其中：

Ω ( ω ) = $− \[ ω$ × ω − ω T 0 ] \boldsymbol{\Omega}(\boldsymbol{\omega}) = \begin{bmatrix} - $\\boldsymbol{\\omega}$ _\times & \boldsymbol{\omega} \\ -\boldsymbol{\omega}^T & 0 \end{bmatrix} Ω(ω)= $−\[ω$ ×−ωTω0]

速度：

v ˙ = R W I T ( a m − b a ) + g \dot{\mathbf{v}} = \mathbf{R}_{WI}^T (\mathbf{a}_m - \mathbf{b}_a) + \mathbf{g} v˙=RWIT(am−ba)+g

位置：

p ˙ = v \dot{\mathbf{p}} = \mathbf{v} p˙=v

偏置（随机游走模型）：

b ˙ g = n b g , b ˙ a = n b a \dot{\mathbf{b}}g = \mathbf{n}{bg}, \quad \dot{\mathbf{b}}a = \mathbf{n}{ba} b˙g=nbg,b˙a=nba

2. 数值积分方法

2.1 欧拉方法（一阶）

最简单的数值积分方法：

x k + 1 = x k + Δ t ⋅ f ( x k , u k ) \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta t \cdot f(\mathbf{x}_k, \mathbf{u}_k) xk+1=xk+Δt⋅f(xk,uk)

优点：实现简单，计算量小

缺点：精度低，需要很小的时间步长

2.2 中点方法（二阶）

改进的积分方法：

k 1 = f ( x k , u k ) \mathbf{k}_1 = f(\mathbf{x}_k, \mathbf{u}_k) k1=f(xk,uk)

k 2 = f ( x k + Δ t 2 k 1 , u k + 1 / 2 ) \mathbf{k}_2 = f\left(\mathbf{x}_k + \frac{\Delta t}{2} \mathbf{k}1, \mathbf{u}{k+1/2}\right) k2=f(xk+2Δtk1,uk+1/2)

x k + 1 = x k + Δ t ⋅ k 2 \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta t \cdot \mathbf{k}_2 xk+1=xk+Δt⋅k2

2.3 龙格库塔方法（四阶）

MSCKF使用的高精度积分方法：

k 1 = f ( t n , y n ) \mathbf{k}_1 = f(t_n, \mathbf{y}_n) k1=f(tn,yn)

k 2 = f ( t n + Δ t 2 , y n + Δ t 2 k 1 ) \mathbf{k}_2 = f\left(t_n + \frac{\Delta t}{2}, \mathbf{y}_n + \frac{\Delta t}{2} \mathbf{k}_1\right) k2=f(tn+2Δt,yn+2Δtk1)

k 3 = f ( t n + Δ t 2 , y n + Δ t 2 k 2 ) \mathbf{k}_3 = f\left(t_n + \frac{\Delta t}{2}, \mathbf{y}_n + \frac{\Delta t}{2} \mathbf{k}_2\right) k3=f(tn+2Δt,yn+2Δtk2)

k 4 = f ( t n + Δ t , y n + Δ t ⋅ k 3 ) \mathbf{k}_4 = f(t_n + \Delta t, \mathbf{y}_n + \Delta t \cdot \mathbf{k}_3) k4=f(tn+Δt,yn+Δt⋅k3)

y n + 1 = y n + Δ t 6 ( k 1 + 2 k 2 + 2 k 3 + k 4 ) \mathbf{y}_{n+1} = \mathbf{y}_n + \frac{\Delta t}{6}(\mathbf{k}_1 + 2\mathbf{k}_2 + 2\mathbf{k}_3 + \mathbf{k}_4) yn+1=yn+6Δt(k1+2k2+2k3+k4)

精度：局部截断误差为 O ( Δ t 5 ) O(\Delta t^5) O(Δt5)

3. 代码实现

3.1 批量IMU处理

cpp 复制代码

// src/msckf_vio.cpp:508-538
void MsckfVio::batchImuProcessing(const double& time_bound) {
    int used_imu_msg_cntr = 0;
    
    for (const auto& imu_msg : imu_msg_buffer) {
        double imu_time = imu_msg.header.stamp.toSec();
        
        // 跳过过时的IMU数据
        if (imu_time < state_server.imu_state.time) {
            ++used_imu_msg_cntr;
            continue;
        }
        
        // 超过目标时间则停止
        if (imu_time > time_bound) break;
        
        // 转换IMU数据
        Vector3d m_gyro, m_acc;
        tf::vectorMsgToEigen(imu_msg.angular_velocity, m_gyro);
        tf::vectorMsgToEigen(imu_msg.linear_acceleration, m_acc);
        
        // 执行过程模型
        processModel(imu_time, m_gyro, m_acc);
        ++used_imu_msg_cntr;
    }
    
    // 设置新的IMU状态ID
    state_server.imu_state.id = IMUState::next_id++;
    
    // 移除已使用的IMU数据
    imu_msg_buffer.erase(imu_msg_buffer.begin(),
        imu_msg_buffer.begin() + used_imu_msg_cntr);
}

3.2 过程模型

cpp 复制代码

// src/msckf_vio.cpp:540-628
void MsckfVio::processModel(const double& time,
    const Vector3d& m_gyro,
    const Vector3d& m_acc) {
    
    // 1. 去除偏置
    IMUState& imu_state = state_server.imu_state;
    Vector3d gyro = m_gyro - imu_state.gyro_bias;
    Vector3d acc = m_acc - imu_state.acc_bias;
    double dtime = time - imu_state.time;
    
    // 2. 计算连续时间系统矩阵F和噪声输入矩阵G
    Matrix<double, 21, 21> F = Matrix<double, 21, 21>::Zero();
    Matrix<double, 21, 12> G = Matrix<double, 21, 12>::Zero();
    
    // F矩阵各项
    F.block<3, 3>(0, 0) = -skewSymmetric(gyro);           // ∂θ̇/∂θ
    F.block<3, 3>(0, 3) = -Matrix3d::Identity();          // ∂θ̇/∂b_g
    F.block<3, 3>(6, 0) = -R.transpose()*skewSymmetric(acc); // ∂v̇/∂θ
    F.block<3, 3>(6, 9) = -R.transpose();                 // ∂v̇/∂b_a
    F.block<3, 3>(12, 6) = Matrix3d::Identity();          // ∂ṗ/∂v
    
    // G矩阵各项
    G.block<3, 3>(0, 0) = -Matrix3d::Identity();          // n_g
    G.block<3, 3>(3, 3) = Matrix3d::Identity();           // n_bg
    G.block<3, 3>(6, 6) = -R.transpose();                 // n_v
    G.block<3, 3>(9, 9) = Matrix3d::Identity();           // n_ba
    
    // 3. 计算状态转移矩阵（泰勒展开近似）
    Matrix<double, 21, 21> Fdt = F * dtime;
    Matrix<double, 21, 21> Fdt_square = Fdt * Fdt;
    Matrix<double, 21, 21> Fdt_cube = Fdt_square * Fdt;
    
    Matrix<double, 21, 21> Phi = Matrix<double, 21, 21>::Identity() +
        Fdt + 0.5*Fdt_square + (1.0/6.0)*Fdt_cube;
    
    // 4. 使用RK4积分更新状态
    predictNewState(dtime, gyro, acc);
    
    // 5. 传播协方差
    // ... (详见协方差传播文档)
}

3.3 龙格库塔积分实现

cpp 复制代码

// src/msckf_vio.cpp:630-693
void MsckfVio::predictNewState(const double& dt,
    const Vector3d& gyro,
    const Vector3d& acc) {
    
    // 获取状态变量引用
    Vector4d& q = state_server.imu_state.orientation;
    Vector3d& v = state_server.imu_state.velocity;
    Vector3d& p = state_server.imu_state.position;
    
    // ===== 姿态更新 =====
    double gyro_norm = gyro.norm();
    Matrix4d Omega = Matrix4d::Zero();
    Omega.block<3, 3>(0, 0) = -skewSymmetric(gyro);
    Omega.block<3, 1>(0, 3) = gyro;
    Omega.block<1, 3>(3, 0) = -gyro;
    
    // 四元数微分方程的解析解
    Vector4d dq_dt, dq_dt2;
    if (gyro_norm > 1e-5) {
        // 精确解
        dq_dt = (cos(gyro_norm*dt*0.5)*Matrix4d::Identity() +
            1/gyro_norm*sin(gyro_norm*dt*0.5)*Omega) * q;
        dq_dt2 = (cos(gyro_norm*dt*0.25)*Matrix4d::Identity() +
            1/gyro_norm*sin(gyro_norm*dt*0.25)*Omega) * q;
    } else {
        // 小角度近似
        dq_dt = (Matrix4d::Identity()+0.5*dt*Omega) *
            cos(gyro_norm*dt*0.5) * q;
        dq_dt2 = (Matrix4d::Identity()+0.25*dt*Omega) *
            cos(gyro_norm*dt*0.25) * q;
    }
    
    // 计算旋转矩阵
    Matrix3d dR_dt_transpose = quaternionToRotation(dq_dt).transpose();
    Matrix3d dR_dt2_transpose = quaternionToRotation(dq_dt2).transpose();
    
    // ===== 速度和位置更新（RK4） =====
    
    // k1 = f(tn, yn)
    Vector3d k1_v_dot = quaternionToRotation(q).transpose()*acc +
        IMUState::gravity;
    Vector3d k1_p_dot = v;
    
    // k2 = f(tn+dt/2, yn+k1*dt/2)
    Vector3d k1_v = v + k1_v_dot*dt/2;
    Vector3d k2_v_dot = dR_dt2_transpose*acc + IMUState::gravity;
    Vector3d k2_p_dot = k1_v;
    
    // k3 = f(tn+dt/2, yn+k2*dt/2)
    Vector3d k2_v = v + k2_v_dot*dt/2;
    Vector3d k3_v_dot = dR_dt2_transpose*acc + IMUState::gravity;
    Vector3d k3_p_dot = k2_v;
    
    // k4 = f(tn+dt, yn+k3*dt)
    Vector3d k3_v = v + k3_v_dot*dt;
    Vector3d k4_v_dot = dR_dt_transpose*acc + IMUState::gravity;
    Vector3d k4_p_dot = k3_v;
    
    // 最终更新
    q = dq_dt;
    quaternionNormalize(q);
    v = v + dt/6*(k1_v_dot + 2*k2_v_dot + 2*k3_v_dot + k4_v_dot);
    p = p + dt/6*(k1_p_dot + 2*k2_p_dot + 2*k3_p_dot + k4_p_dot);
}

4. 数学推导

4.1 四元数微分方程求解

四元数微分方程：

q ˙ = 1 2 Ω ( ω ) q \dot{\mathbf{q}} = \frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}) \mathbf{q} q˙=21Ω(ω)q

对于恒定角速度 ω \boldsymbol{\omega} ω，解析解为：

q ( t ) = exp ⁡ ( 1 2 Ω t ) q ( 0 ) \mathbf{q}(t) = \exp\left(\frac{1}{2} \boldsymbol{\Omega} t\right) \mathbf{q}(0) q(t)=exp(21Ωt)q(0)

其中矩阵指数：

exp ⁡ ( 1 2 Ω t ) = cos ⁡ ( ∥ ω ∥ t 2 ) I + 1 ∥ ω ∥ sin ⁡ ( ∥ ω ∥ t 2 ) Ω \exp\left(\frac{1}{2} \boldsymbol{\Omega} t\right) = \cos\left(\frac{\|\boldsymbol{\omega}\|t}{2}\right)\mathbf{I} + \frac{1}{\|\boldsymbol{\omega}\|}\sin\left(\frac{\|\boldsymbol{\omega}\|t}{2}\right)\boldsymbol{\Omega} exp(21Ωt)=cos(2∥ω∥t)I+∥ω∥1sin(2∥ω∥t)Ω

4.2 速度和位置积分

速度微分方程 ：

v ˙ = R T a + g \dot{\mathbf{v}} = \mathbf{R}^T \mathbf{a} + \mathbf{g} v˙=RTa+g

位置微分方程 ：

p ˙ = v \dot{\mathbf{p}} = \mathbf{v} p˙=v

使用RK4积分：

k 1 v = R n T a n + g \mathbf{k}_{1v} = \mathbf{R}_n^T \mathbf{a}_n + \mathbf{g} k1v=RnTan+g

k 1 p = v n \mathbf{k}_{1p} = \mathbf{v}_n k1p=vn

k 2 v = R n + 1 / 2 T a n + g \mathbf{k}{2v} = \mathbf{R}{n+1/2}^T \mathbf{a}_n + \mathbf{g} k2v=Rn+1/2Tan+g

k 2 p = v n + Δ t 2 k 1 v \mathbf{k}_{2p} = \mathbf{v}n + \frac{\Delta t}{2} \mathbf{k}{1v} k2p=vn+2Δtk1v

k 3 v = R n + 1 / 2 T a n + g \mathbf{k}{3v} = \mathbf{R}{n+1/2}^T \mathbf{a}_n + \mathbf{g} k3v=Rn+1/2Tan+g

k 3 p = v n + Δ t 2 k 2 v \mathbf{k}_{3p} = \mathbf{v}n + \frac{\Delta t}{2} \mathbf{k}{2v} k3p=vn+2Δtk2v

k 4 v = R n + 1 T a n + g \mathbf{k}{4v} = \mathbf{R}{n+1}^T \mathbf{a}_n + \mathbf{g} k4v=Rn+1Tan+g

k 4 p = v n + Δ t ⋅ k 3 v \mathbf{k}_{4p} = \mathbf{v}n + \Delta t \cdot \mathbf{k}{3v} k4p=vn+Δt⋅k3v

最终更新：

v n + 1 = v n + Δ t 6 ( k 1 v + 2 k 2 v + 2 k 3 v + k 4 v ) \mathbf{v}{n+1} = \mathbf{v}n + \frac{\Delta t}{6}(\mathbf{k}{1v} + 2\mathbf{k}{2v} + 2\mathbf{k}{3v} + \mathbf{k}{4v}) vn+1=vn+6Δt(k1v+2k2v+2k3v+k4v)

p n + 1 = p n + Δ t 6 ( k 1 p + 2 k 2 p + 2 k 3 p + k 4 p ) \mathbf{p}{n+1} = \mathbf{p}n + \frac{\Delta t}{6}(\mathbf{k}{1p} + 2\mathbf{k}{2p} + 2\mathbf{k}{3p} + \mathbf{k}{4p}) pn+1=pn+6Δt(k1p+2k2p+2k3p+k4p)

5. 系统矩阵详解

5.1 F矩阵（系统矩阵）

F矩阵描述了状态变量之间的耦合关系：

F = $\partial θ ˙ \partial θ \partial θ ˙ \partial b g 0 0 0 0 0 0 0 0 0 0 0 0 \partial v ˙ \partial θ 0 0 \partial v ˙ \partial b a 0 0 0 0 0 0 0 0 0 0 0 0 \partial p ˙ \partial v 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0$ F = \begin{bmatrix} \frac{\partial \dot{\theta}}{\partial \theta} & \frac{\partial \dot{\theta}}{\partial b_g} & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ \frac{\partial \dot{v}}{\partial \theta} & 0 & 0 & \frac{\partial \dot{v}}{\partial b_a} & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & \frac{\partial \dot{p}}{\partial v} & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix} F= ∂θ∂θ˙0∂θ∂v˙0000∂bg∂θ˙0000000000∂v∂p˙0000∂ba∂v˙0000000000000000000000000

各项含义：

项	表达式	物理意义
∂ θ ˙ ∂ θ \frac{\partial \dot{\theta}}{\partial \theta} ∂θ∂θ˙	− $ω$ × - $\\boldsymbol{\\omega}$ _\times − $ω$ ×	姿态-姿态耦合
∂ θ ˙ ∂ b g \frac{\partial \dot{\theta}}{\partial b_g} ∂bg∂θ˙	− I -\mathbf{I} −I	姿态-陀螺仪偏置耦合
∂ v ˙ ∂ θ \frac{\partial \dot{v}}{\partial \theta} ∂θ∂v˙	− R T $a$ × -\mathbf{R}^T $\\mathbf{a}$ _\times −RT $a$ ×	速度-姿态耦合
∂ v ˙ ∂ b a \frac{\partial \dot{v}}{\partial b_a} ∂ba∂v˙	− R T -\mathbf{R}^T −RT	速度-加速度偏置耦合
∂ p ˙ ∂ v \frac{\partial \dot{p}}{\partial v} ∂v∂p˙	I \mathbf{I} I	位置-速度耦合

5.2 G矩阵（噪声输入矩阵）

G矩阵描述了噪声如何影响状态：

G = $- I 0 0 0 0 I 0 0 0 0 - R T 0 0 0 0 I 0 0 0 0 0 0 0 0 0 0 0 0$ G = \begin{bmatrix} -\mathbf{I} & 0 & 0 & 0 \\ 0 & \mathbf{I} & 0 & 0 \\ 0 & 0 & -\mathbf{R}^T & 0 \\ 0 & 0 & 0 & \mathbf{I} \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{bmatrix} G= −I0000000I0000000−RT0000000I000

对应噪声向量：

n = $n g n b g n v n b a$ \mathbf{n} = \begin{bmatrix} \mathbf{n}g \\ \mathbf{n}{bg} \\ \mathbf{n}v \\ \mathbf{n}{ba} \end{bmatrix} n= ngnbgnvnba

6. 实现细节

6.1 时间同步

cpp 复制代码

// 确保IMU数据按时间顺序处理
if (imu_time < state_server.imu_state.time) {
    ++used_imu_msg_cntr;
    continue;  // 跳过过时数据
}

6.2 偏置去除

cpp 复制代码

// 去除偏置后的测量值
Vector3d gyro = m_gyro - imu_state.gyro_bias;
Vector3d acc = m_acc - imu_state.acc_bias;

6.3 四元数归一化

cpp 复制代码

// 积分后归一化四元数
q = dq_dt;
quaternionNormalize(q);

重要：数值积分会引入误差，导致四元数偏离单位长度，必须定期归一化。

6.4 数值稳定性

问题：当 ∥ ω ∥ \|\boldsymbol{\omega}\| ∥ω∥ 很小时， sin ⁡ ( ∥ ω ∥ t / 2 ) / ∥ ω ∥ \sin(\|\boldsymbol{\omega}\|t/2)/\|\boldsymbol{\omega}\| sin(∥ω∥t/2)/∥ω∥ 计算不稳定。

解决方案：

cpp 复制代码

if (gyro_norm > 1e-5) {
    // 使用精确公式
    dq_dt = (cos(gyro_norm*dt*0.5)*I + 
        sin(gyro_norm*dt*0.5)/gyro_norm*Omega) * q;
} else {
    // 使用泰勒展开近似
    dq_dt = (I + 0.5*dt*Omega) * cos(gyro_norm*dt*0.5) * q;
}

7. 性能优化

7.1 计算复杂度

F矩阵计算： O ( 1 ) O(1) O(1)
矩阵指数近似： O ( 1 ) O(1) O(1)（固定21×21矩阵）
RK4积分： O ( 1 ) O(1) O(1)
总体： O ( 1 ) O(1) O(1) 每个IMU数据点

7.2 内存使用

临时矩阵：约 3.5KB（21×21 double矩阵）
状态变量：21个double

8. 调试技巧

8.1 验证积分精度

cpp 复制代码

// 比较欧拉法和RK4结果
Vector3d euler_v = v + dt * (R.transpose()*acc + gravity);
Vector3d rk4_v = v + dt/6*(k1_v_dot + 2*k2_v_dot + 2*k3_v_dot + k4_v_dot);

double error = (euler_v - rk4_v).norm();
if (error > threshold) {
    ROS_WARN("Integration method difference: %f", error);
}

8.2 状态变化监控

cpp 复制代码

// 监控状态变化是否合理
if (delta_v.norm() > max_velocity_change) {
    ROS_WARN("Large velocity change: %f m/s", delta_v.norm());
}
if (delta_p.norm() > max_position_change) {
    ROS_WARN("Large position change: %f m", delta_p.norm());
}

9. 常见问题

9.1 积分发散

症状：状态值快速增长到不合理范围

原因：

IMU数据包含异常值
偏置估计不准确
时间戳错误

解决：

cpp 复制代码

// 添加异常值检测
if (m_gyro.norm() > max_gyro_threshold || 
    m_acc.norm() > max_acc_threshold) {
    ROS_WARN("IMU outlier detected, skipping...");
    continue;
}

9.2 姿态漂移

症状：估计的重力方向逐渐偏离真实方向

原因：陀螺仪偏置累积

解决：通过测量更新持续修正偏置

10. 总结

状态传播是MSCKF VIO的核心步骤之一，通过本文的介绍，读者应该了解：

连续时间系统模型的建立
四阶龙格库塔积分的实现
系统矩阵F和G的计算
数值稳定性的保证方法

下一篇预告

下一篇文章将详细介绍协方差传播过程，包括过程噪声的建模和协方差矩阵的更新。

参考文献

Trawny, N., & Roumeliotis, S. I. (2005). Indirect Kalman filter for 3D attitude estimation. University of Minnesota, Dept. of Comp. Sci. & Eng., Tech. Rep.
Butcher, J. C. (2016). Numerical methods for ordinary differential equations. John Wiley & Sons.

本文为原创文章，转载请注明出处。欢迎点赞、收藏、评论交流！

[MSCKF-VIO]状态传播：四阶龙格库塔积分

状态传播：四阶龙格库塔积分

文章简介

目录

文章目录

1. 状态传播概述

1.1 预测步骤的作用

1.2 连续时间系统模型

2. 数值积分方法

2.1 欧拉方法（一阶）

2.2 中点方法（二阶）

2.3 龙格库塔方法（四阶）

3. 代码实现

3.1 批量IMU处理

3.2 过程模型

3.3 龙格库塔积分实现

4. 数学推导

4.1 四元数微分方程求解

4.2 速度和位置积分

5. 系统矩阵详解

5.1 F矩阵（系统矩阵）

5.2 G矩阵（噪声输入矩阵）

6. 实现细节

6.1 时间同步

6.2 偏置去除

6.3 四元数归一化

6.4 数值稳定性

7. 性能优化

7.1 计算复杂度

7.2 内存使用

8. 调试技巧

8.1 验证积分精度

8.2 状态变化监控

9. 常见问题

9.1 积分发散

9.2 姿态漂移

10. 总结

下一篇预告

参考文献