FA_融合和滤波(FF)-误差状态卡尔曼滤波(ESKF)示例二

田间的水稻

78人浏览 · 2026-09-30 09:24:48

田间的水稻 · 2026-09-30 09:24:48 发布

FA：formulas and algorithm, FF：fusion and filtering，ESKF：（Error State Kalman Filter）

ESKF（误差状态卡尔曼滤波）IMU+GNSS 15 维 C++ 示例

状态维度说明：15 维误差状态 $δx∈R15\delta \mathbf{x} \in \mathbb{R}^{15}$

位置误差 $δp\delta\mathbf{p}$ ：3 维
速度误差 $δv\delta\mathbf{v}$ ：3 维
姿态微小旋转误差 $δθ\delta\boldsymbol{\theta}$ ：3 维（SO(3) 切空间）
加速度计偏置误差 $δba\delta\mathbf{b}_a$ ：3 维
陀螺仪偏置误差 $δbg\delta\mathbf{b}_g$ ：3 维

名义状态： $xn=[p,v,q,ba,bg]\mathbf{x}_n = [\mathbf{p},\mathbf{v},\mathbf{q},\mathbf{b}_a,\mathbf{b}_g]$ ，四元数 4 维，名义状态一共 17 维；误差状态 15 维（ESKF 核心特点：姿态用 3 维小量，不是 4 维四元数误差）

依赖：Eigen3（线性代数，机器人导航标配，ubuntu 直接apt install libeigen3-dev），无需其他第三方库，复制即可编译运行。

一、ESKF 原理简要（IMU+GNSS）

两个阶段

1. 预测阶段（IMU 驱动，高频）

IMU 加速度、角速度积分更新名义状态；同时传播误差状态协方差矩阵 $P$ 。
$δx˙=Fcδx+Gcw\delta\dot{\mathbf{x}} = \mathbf{F}_c \delta\mathbf{x}+\mathbf{G}_c \mathbf{w}$
离散：
$δxk+1=Fδxk+GwkPk+1∣k=FPk∣kFT+GQGT\delta\mathbf{x}_{k+1} = \mathbf{F}\delta\mathbf{x}_k+\mathbf{G}\mathbf{w}_k \mathbf{P}_{k+1|k} = \mathbf{F}\mathbf{P}_{k|k}\mathbf{F}^T + \mathbf{G}\mathbf{Q}\mathbf{G}^T$

Q：IMU 噪声对角阵（加速度噪声、陀螺噪声、偏置随机游走）

2. 更新阶段（GNSS 位置观测，低频）

GNSS 位置到达，计算残差、观测雅可比、卡尔曼增益，更新误差状态；然后把误差叠加到名义状态上，误差状态重置为 0（ESKF 关键！）
$r=z−h(xnominal)K=Pk∣k−1HT(HPk∣k−1HT+R)−1\mathbf{r}= \mathbf{z}-h(\mathbf{x}_{nominal}) \mathbf{K}= \mathbf{P}_{k|k-1}\mathbf{H}^T\left(\mathbf{H}\mathbf{P}_{k|k-1}\mathbf{H}^T+\mathbf{R}\right)^{-1}$
$δx^=Kr\delta\hat{\mathbf{x}} = \mathbf{K}\mathbf{r}$

Joseph 协方差更新（保证正定）：
$Pk∣k=(I−KH)Pk∣k−1(I−KH)T+KRKT\mathbf{P}_{k|k}=(\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}_{k|k-1}(\mathbf{I}-\mathbf{K}\mathbf{H})^T+\mathbf{K}\mathbf{R}\mathbf{K}^T$

二、完整 C++ 代码（Eigen，直接编译）

#include <Eigen/Dense>
#include <iostream>
#include <cmath>

using namespace Eigen;
using namespace std;

// ===================== ESKF 15维误差状态 IMU+GNSS =====================
// 误差状态顺序： dp(3), dv(3), dtheta(3), dba(3), dbg(3)  total:15
struct NominalState {
    Vector3d p;      // 位置 nominal
    Vector3d v;      // 速度 nominal
    Quaterniond q;   // 姿态四元数 q_wxyz
    Vector3d ba;     // 加速度计偏置 nominal
    Vector3d bg;     // 陀螺仪偏置 nominal

    NominalState() {
        p.setZero();
        v.setZero();
        q = Quaterniond::Identity();
        ba.setZero();
        bg.setZero();
    }
};

class ESKF_IMU_GNSS {
public:
    NominalState x_n;                // 名义状态
    Matrix<double,15,15> P;          // 误差协方差 P 15x15
    Matrix<double,15,15> I15;        // 15阶单位阵

    // 噪声参数，根据IMU标定修改
    double sigma_a;     // 加速度噪声 m/s^2
    double sigma_g;     // 陀螺噪声 rad/s
    double sigma_ba;    // ba随机游走
    double sigma_bg;    // bg随机游走
    double sigma_gnss;  // GNSS位置观测噪声 m

    ESKF_IMU_GNSS() {
        I15.setIdentity();
        P.setZero();
        // 初始化协方差
        P.block<3,3>(0,0) = Matrix3d::Identity() * 0.1;
        P.block<3,3>(3,3) = Matrix3d::Identity() * 0.1;
        P.block<3,3>(6,6) = Matrix3d::Identity() * 0.01;
        P.block<3,3>(9,9) = Matrix3d::Identity() * 1e-4;
        P.block<3,3>(12,12) = Matrix3d::Identity() * 1e-4;

        // 默认噪声参数
        sigma_a = 0.05;
        sigma_g = 0.01;
        sigma_ba = 0.001;
        sigma_bg = 0.0001;
        sigma_gnss = 0.2;
    }

    // ========================= 预测：IMU积分 =========================
    // imu_acc: 机体坐标系加速度； imu_gyro:机体角速度； dt:时间步长
    void Predict(const Vector3d& imu_acc, const Vector3d& imu_gyro, double dt) {
        // 1. 修正IMU测量值，减去偏置
        Vector3d acc = imu_acc - x_n.ba;
        Vector3d gyro = imu_gyro - x_n.bg;
        Vector3d g(0,0,9.81); // 重力向量(NED系)

        // ---------- 更新名义状态 ----------
        // 姿态积分 四元数更新
        Quaterniond dq;
        Vector3d w_dt = gyro * dt;
        double theta = w_dt.norm();
        if(theta < 1e-6) {
            dq = Quaterniond(1, 0.5*w_dt(0), 0.5*w_dt(1), 0.5*w_dt(2));
        } else {
            dq.w() = cos(0.5*theta);
            dq.vec() = sin(0.5*theta)/theta * w_dt;
        }
        x_n.q = (x_n.q * dq).normalized();

        // 速度、位置积分
        Matrix3d R = x_n.q.toRotationMatrix();
        Vector3d acc_world = R * acc - g;
        x_n.v += acc_world * dt;
        x_n.p += x_n.v * dt + 0.5 * acc_world * dt * dt;

        // ---------- 离散状态转移矩阵 F 15x15 ----------
        Matrix<double,15,15> F;
        F.setIdentity();
        F.block<3,3>(0,3) = Matrix3d::Identity() * dt;
        F.block<3,3>(3,6) = -R * skew(acc) * dt;
        F.block<3,3>(3,9) = -R * dt;
        F.block<3,3>(6,6) = Matrix3d::Identity() - skew(gyro)*dt;
        F.block<3,3>(6,12) = -Matrix3d::Identity() * dt;

        // ---------- 噪声输入矩阵 G 15x12 ----------
        Matrix<double,15,12> G;
        G.setZero();
        G.block<3,3>(3,0) = -R*dt;
        G.block<3,3>(6,3) = -Matrix3d::Identity()*dt;
        G.block<3,3>(9,6) = Matrix3d::Identity()*dt;
        G.block<3,3>(12,9) = Matrix3d::Identity()*dt;

        // ---------- 噪声协方差 Q 12x12 ----------
        Matrix<double,12,12> Q;
        Q.setZero();
        Q.block<3,3>(0,0) = Matrix3d::Identity() * sigma_a*sigma_a*dt;
        Q.block<3,3>(3,3) = Matrix3d::Identity() * sigma_g*sigma_g*dt;
        Q.block<3,3>(6,6) = Matrix3d::Identity() * sigma_ba*sigma_ba*dt;
        Q.block<3,3>(9,9) = Matrix3d::Identity() * sigma_bg*sigma_bg*dt;

        // 协方差传播 P = F*P*F^T + G*Q*G^T
        P = F * P * F.transpose() + G * Q * G.transpose();
        // 保证对称，防止数值漂移
        P = (P + P.transpose()) / 2.0;
    }

    // ========================= 更新：GNSS位置观测 =========================
    // z_gnss: GNSS 世界坐标系位置观测值 (3维)
    void UpdateGNSS(const Vector3d& z_gnss) {
        // 观测残差 r = z - h(x_n), h就是名义位置
        Vector3d r = z_gnss - x_n.p;

        // 观测雅可比 H 3×15
        Matrix<double,3,15> H;
        H.setZero();
        H.block<3,3>(0,0) = Matrix3d::Identity(); // 观测只对位置误差有关

        // 观测噪声 R
        Matrix3d R = Matrix3d::Identity() * sigma_gnss * sigma_gnss;

        // 卡尔曼增益 K
        Matrix<double,15,3> K = P * H.transpose() * (H * P * H.transpose() + R).inverse();

        // 误差状态增量 dx
        Matrix<double,15,1> dx = K * r;

        // ========== 误差叠加到名义状态 ESKF核心：⊕操作 ==========
        // dp
        x_n.p += dx.block<3,1>(0,0);
        // dv
        x_n.v += dx.block<3,1>(3,0);
        // dtheta 姿态增量：四元数左乘微小旋转
        Vector3d dtheta = dx.block<3,1>(6,0);
        Quaterniond dq;
        double theta = dtheta.norm();
        if(theta <1e-6){
            dq = Quaterniond(1, 0.5*dtheta(0),0.5*dtheta(1),0.5*dtheta(2));
        }else{
            dq.w() = cos(theta/2.0);
            dq.vec() = sin(theta/2.0)/theta * dtheta;
        }
        x_n.q = (dq * x_n.q).normalized();
        // ba bg偏置更新
        x_n.ba += dx.block<3,1>(9,0);
        x_n.bg += dx.block<3,1>(12,0);

        // ========== Joseph形式更新协方差，保证正定 ==========
        Matrix<double,15,15> I_KH = I15 - K*H;
        P = I_KH * P * I_KH.transpose() + K * R * K.transpose();
        P = (P + P.transpose()) / 2.0;

        // ESKF：误差状态dx重置为0（不需要保存dx，用完就叠加进名义状态）
    }

    // 辅助函数：反对称矩阵
    static Matrix3d skew(const Vector3d& v) {
        Matrix3d m;
        m << 0, -v(2), v(1),
             v(2), 0, -v(0),
             -v(1), v(0), 0;
        return m;
    }
};

// ======================== 测试主函数 ========================
int main() {
    ESKF_IMU_GNSS eskf;
    double dt = 0.01; // IMU 100Hz
    Vector3d imu_acc(0,0,9.81); // 静止IMU，只有重力
    Vector3d imu_gyro(0,0,0);

    cout << "==== ESKF IMU+GNSS 15维测试 ====" << endl;
    // 模拟IMU预测100次
    for(int i=0;i<100;i++){
        eskf.Predict(imu_acc, imu_gyro, dt);
        // 每20帧模拟一次GNSS观测（5Hz GNSS）
        if(i%20 == 0){
            Vector3d gnss_z(0,0,0); // GNSS观测真值
            eskf.UpdateGNSS(gnss_z);
            cout << "time: "<<i*dt << " pos: "<<eskf.x_n.p.transpose() <<endl;
        }
    }
    return 0;
}

编译命令

g++ eskf_imu_gnss.cpp -o eskf -I/usr/include/eigen3 -O2
./eskf

三、ESKF（IMU+GNSS）优缺点

优点

误差状态是 15 维向量（线性空间），姿态使用 3 维切空间小量，避免四元数 4 维冗余导致协方差奇异，这是 ESKF 对比标准 EKF 最大优势。标准 EKF 直接估计四元数，4 维姿态自由度冗余，协方差容易发散。
IMU 预测高频（100~500Hz），GNSS 低频更新，完美适配组合导航传感器异步特性。
数值稳定性更好：误差都是小量，雅可比矩阵线性近似精度高；每次更新后误差状态重置，防止误差累积。
偏置在线估计：加速度计、陀螺仪 bias 实时估计，不需要提前精细标定。
工程落地广泛：VIO、LIO、车载组合导航（GNSS+IMU）主流方案（GTSAM、VINS-Mono 都采用 ESKF 思想）。

缺点