企业官网建设流程全解析

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}δx∈R15

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

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

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

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

两个阶段

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

IMU 加速度、角速度积分更新名义状态；同时传播误差状态协方差矩阵PPP。
δx˙=Fcδx+Gcw\delta\dot{\mathbf{x}} = \mathbf{F}_c \delta\mathbf{x}+\mathbf{G}_c \mathbf{w}δx˙=Fc​δx+Gc​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δxk+1​=Fδxk​+Gwk​Pk+1∣k​=FPk∣k​FT+GQGT

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}r=z−h(xnominal​)K=Pk∣k−1​HT(HPk∣k−1​HT+R)−1
δx^=Kr\delta\hat{\mathbf{x}} = \mathbf{K}\mathbf{r}δx^=Kr

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}^TPk∣k​=(I−KH)Pk∣k−1​(I−KH)T+KRKT

二、完整 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）优缺点

优点

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

缺点

1. 属于卡尔曼框架，强依赖高斯噪声假设；GNSS 出现粗差（多路径、跳变）时没有鲁棒性，容易滤波发散。工程上需要额外增加异常检测（残差卡方检验）。
2. 仍然是一阶线性近似，运动剧烈、大角度旋转时，线性误差变大。
3. IMU 积分会随时间漂移，必须依赖外部观测（GNSS）持续修正，长时间无 GNSS 信号（隧道、室内）定位漂移快速增长。
4. 需要仔细调参：噪声矩阵(Q,R)对结果影响极大；IMU 噪声参数需要实际标定，凭经验设置会效果很差。
5. 实现细节坑多：四元数归一化、协方差矩阵强制对称、姿态增量左乘 / 右乘（坐标系 NED/ENU 容易搞混）。

四、代码说明 & 工程扩展提示

1. 坐标系：当前代码使用NED（北东地），如果你需要 ENU，重力向量改为Vector3d g(0,0,-9.81)，姿态矩阵部分对应修改。
2. 噪声参数：sigma_a sigma_g sigma_ba sigma_bg需要通过 IMU Allan 方差标定得到。
3. GNSS 观测：当前只使用位置观测；如果需要 GNSS 速度观测，可以扩展观测雅可比 H 矩阵，增加速度残差。
4. 鲁棒改进：增加残差卡方检测，剔除 GNSS 野值；可以切换 UKF 或者加入滑动窗口因子图优化（GTSAM）。
5. 扩展：可以直接增加激光 / 视觉观测，只需要新增观测方程和观测雅可比 H。