-误差状态卡尔曼滤波(ESKF)示例二)
FAformulas and algorithm,FFfusion and filteringESKFError State Kalman FilterESKF误差状态卡尔曼滤波IMUGNSS 15 维 C 示例状态维度说明15 维误差状态δx∈R15\delta \mathbf{x} \in \mathbb{R}^{15}δx∈R15位置误差δp\delta\mathbf{p}δp3 维速度误差δv\delta\mathbf{v}δv3 维姿态微小旋转误差δθ\delta\boldsymbol{\theta}δθ3 维SO(3) 切空间加速度计偏置误差δba\delta\mathbf{b}_aδba3 维陀螺仪偏置误差δbg\delta\mathbf{b}_gδbg3 维名义状态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 原理简要IMUGNSS两个阶段1.预测阶段IMU 驱动高频IMU 加速度、角速度积分更新名义状态同时传播误差状态协方差矩阵PPP。δx˙FcδxGcw\delta\dot{\mathbf{x}} \mathbf{F}_c \delta\mathbf{x}\mathbf{G}_c \mathbf{w}δx˙FcδxGcw离散δxk1FδxkGwkPk1∣kFPk∣kFTGQGT\delta\mathbf{x}_{k1} \mathbf{F}\delta\mathbf{x}_k\mathbf{G}\mathbf{w}_k \mathbf{P}_{k1|k} \mathbf{F}\mathbf{P}_{k|k}\mathbf{F}^T \mathbf{G}\mathbf{Q}\mathbf{G}^Tδxk1FδxkGwkPk1∣kFPk∣kFTGQGTQIMU 噪声对角阵加速度噪声、陀螺噪声、偏置随机游走2.更新阶段GNSS 位置观测低频GNSS 位置到达计算残差、观测雅可比、卡尔曼增益更新误差状态然后把误差叠加到名义状态上误差状态重置为 0ESKF 关键rz−h(xnominal)KPk∣k−1HT(HPk∣k−1HTR)−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}rz−h(xnominal)KPk∣k−1HT(HPk∣k−1HTR)−1δx^Kr\delta\hat{\mathbf{x}} \mathbf{K}\mathbf{r}δx^KrJoseph 协方差更新保证正定Pk∣k(I−KH)Pk∣k−1(I−KH)TKRKT\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)TKRKT二、完整 C 代码Eigen直接编译#include Eigen/Dense #include iostream #include cmath using namespace Eigen; using namespace std; // ESKF 15维误差状态 IMUGNSS // 误差状态顺序 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; // 名义状态 Matrixdouble,15,15 P; // 误差协方差 P 15x15 Matrixdouble,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.block3,3(0,0) Matrix3d::Identity() * 0.1; P.block3,3(3,3) Matrix3d::Identity() * 0.1; P.block3,3(6,6) Matrix3d::Identity() * 0.01; P.block3,3(9,9) Matrix3d::Identity() * 1e-4; P.block3,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 ---------- Matrixdouble,15,15 F; F.setIdentity(); F.block3,3(0,3) Matrix3d::Identity() * dt; F.block3,3(3,6) -R * skew(acc) * dt; F.block3,3(3,9) -R * dt; F.block3,3(6,6) Matrix3d::Identity() - skew(gyro)*dt; F.block3,3(6,12) -Matrix3d::Identity() * dt; // ---------- 噪声输入矩阵 G 15x12 ---------- Matrixdouble,15,12 G; G.setZero(); G.block3,3(3,0) -R*dt; G.block3,3(6,3) -Matrix3d::Identity()*dt; G.block3,3(9,6) Matrix3d::Identity()*dt; G.block3,3(12,9) Matrix3d::Identity()*dt; // ---------- 噪声协方差 Q 12x12 ---------- Matrixdouble,12,12 Q; Q.setZero(); Q.block3,3(0,0) Matrix3d::Identity() * sigma_a*sigma_a*dt; Q.block3,3(3,3) Matrix3d::Identity() * sigma_g*sigma_g*dt; Q.block3,3(6,6) Matrix3d::Identity() * sigma_ba*sigma_ba*dt; Q.block3,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 Matrixdouble,3,15 H; H.setZero(); H.block3,3(0,0) Matrix3d::Identity(); // 观测只对位置误差有关 // 观测噪声 R Matrix3d R Matrix3d::Identity() * sigma_gnss * sigma_gnss; // 卡尔曼增益 K Matrixdouble,15,3 K P * H.transpose() * (H * P * H.transpose() R).inverse(); // 误差状态增量 dx Matrixdouble,15,1 dx K * r; // 误差叠加到名义状态 ESKF核心⊕操作 // dp x_n.p dx.block3,1(0,0); // dv x_n.v dx.block3,1(3,0); // dtheta 姿态增量四元数左乘微小旋转 Vector3d dtheta dx.block3,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.block3,1(9,0); x_n.bg dx.block3,1(12,0); // Joseph形式更新协方差保证正定 Matrixdouble,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 IMUGNSS 15维测试 endl; // 模拟IMU预测100次 for(int i0;i100;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三、ESKFIMUGNSS优缺点优点误差状态是 15 维向量线性空间姿态使用 3 维切空间小量避免四元数 4 维冗余导致协方差奇异这是 ESKF 对比标准 EKF 最大优势。标准 EKF 直接估计四元数4 维姿态自由度冗余协方差容易发散。IMU 预测高频100~500HzGNSS 低频更新完美适配组合导航传感器异步特性。数值稳定性更好误差都是小量雅可比矩阵线性近似精度高每次更新后误差状态重置防止误差累积。偏置在线估计加速度计、陀螺仪 bias 实时估计不需要提前精细标定。工程落地广泛VIO、LIO、车载组合导航GNSSIMU主流方案GTSAM、VINS-Mono 都采用 ESKF 思想。缺点属于卡尔曼框架强依赖高斯噪声假设GNSS 出现粗差多路径、跳变时没有鲁棒性容易滤波发散。工程上需要额外增加异常检测残差卡方检验。仍然是一阶线性近似运动剧烈、大角度旋转时线性误差变大。IMU 积分会随时间漂移必须依赖外部观测GNSS持续修正长时间无 GNSS 信号隧道、室内定位漂移快速增长。需要仔细调参噪声矩阵(Q,R)对结果影响极大IMU 噪声参数需要实际标定凭经验设置会效果很差。实现细节坑多四元数归一化、协方差矩阵强制对称、姿态增量左乘 / 右乘坐标系 NED/ENU 容易搞混。四、代码说明 工程扩展提示坐标系当前代码使用NED北东地如果你需要 ENU重力向量改为Vector3d g(0,0,-9.81)姿态矩阵部分对应修改。噪声参数sigma_a sigma_g sigma_ba sigma_bg需要通过 IMU Allan 方差标定得到。GNSS 观测当前只使用位置观测如果需要 GNSS 速度观测可以扩展观测雅可比 H 矩阵增加速度残差。鲁棒改进增加残差卡方检测剔除 GNSS 野值可以切换 UKF 或者加入滑动窗口因子图优化GTSAM。扩展可以直接增加激光 / 视觉观测只需要新增观测方程和观测雅可比 H。
FA_融合和滤波(FF)-误差状态卡尔曼滤波(ESKF)示例二
FAformulas and algorithm,FFfusion and filteringESKFError State Kalman FilterESKF误差状态卡尔曼滤波IMUGNSS 15 维 C 示例状态维度说明15 维误差状态δx∈R15\delta \mathbf{x} \in \mathbb{R}^{15}δx∈R15位置误差δp\delta\mathbf{p}δp3 维速度误差δv\delta\mathbf{v}δv3 维姿态微小旋转误差δθ\delta\boldsymbol{\theta}δθ3 维SO(3) 切空间加速度计偏置误差δba\delta\mathbf{b}_aδba3 维陀螺仪偏置误差δbg\delta\mathbf{b}_gδbg3 维名义状态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 原理简要IMUGNSS两个阶段1.预测阶段IMU 驱动高频IMU 加速度、角速度积分更新名义状态同时传播误差状态协方差矩阵PPP。δx˙FcδxGcw\delta\dot{\mathbf{x}} \mathbf{F}_c \delta\mathbf{x}\mathbf{G}_c \mathbf{w}δx˙FcδxGcw离散δxk1FδxkGwkPk1∣kFPk∣kFTGQGT\delta\mathbf{x}_{k1} \mathbf{F}\delta\mathbf{x}_k\mathbf{G}\mathbf{w}_k \mathbf{P}_{k1|k} \mathbf{F}\mathbf{P}_{k|k}\mathbf{F}^T \mathbf{G}\mathbf{Q}\mathbf{G}^Tδxk1FδxkGwkPk1∣kFPk∣kFTGQGTQIMU 噪声对角阵加速度噪声、陀螺噪声、偏置随机游走2.更新阶段GNSS 位置观测低频GNSS 位置到达计算残差、观测雅可比、卡尔曼增益更新误差状态然后把误差叠加到名义状态上误差状态重置为 0ESKF 关键rz−h(xnominal)KPk∣k−1HT(HPk∣k−1HTR)−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}rz−h(xnominal)KPk∣k−1HT(HPk∣k−1HTR)−1δx^Kr\delta\hat{\mathbf{x}} \mathbf{K}\mathbf{r}δx^KrJoseph 协方差更新保证正定Pk∣k(I−KH)Pk∣k−1(I−KH)TKRKT\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)TKRKT二、完整 C 代码Eigen直接编译#include Eigen/Dense #include iostream #include cmath using namespace Eigen; using namespace std; // ESKF 15维误差状态 IMUGNSS // 误差状态顺序 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; // 名义状态 Matrixdouble,15,15 P; // 误差协方差 P 15x15 Matrixdouble,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.block3,3(0,0) Matrix3d::Identity() * 0.1; P.block3,3(3,3) Matrix3d::Identity() * 0.1; P.block3,3(6,6) Matrix3d::Identity() * 0.01; P.block3,3(9,9) Matrix3d::Identity() * 1e-4; P.block3,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 ---------- Matrixdouble,15,15 F; F.setIdentity(); F.block3,3(0,3) Matrix3d::Identity() * dt; F.block3,3(3,6) -R * skew(acc) * dt; F.block3,3(3,9) -R * dt; F.block3,3(6,6) Matrix3d::Identity() - skew(gyro)*dt; F.block3,3(6,12) -Matrix3d::Identity() * dt; // ---------- 噪声输入矩阵 G 15x12 ---------- Matrixdouble,15,12 G; G.setZero(); G.block3,3(3,0) -R*dt; G.block3,3(6,3) -Matrix3d::Identity()*dt; G.block3,3(9,6) Matrix3d::Identity()*dt; G.block3,3(12,9) Matrix3d::Identity()*dt; // ---------- 噪声协方差 Q 12x12 ---------- Matrixdouble,12,12 Q; Q.setZero(); Q.block3,3(0,0) Matrix3d::Identity() * sigma_a*sigma_a*dt; Q.block3,3(3,3) Matrix3d::Identity() * sigma_g*sigma_g*dt; Q.block3,3(6,6) Matrix3d::Identity() * sigma_ba*sigma_ba*dt; Q.block3,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 Matrixdouble,3,15 H; H.setZero(); H.block3,3(0,0) Matrix3d::Identity(); // 观测只对位置误差有关 // 观测噪声 R Matrix3d R Matrix3d::Identity() * sigma_gnss * sigma_gnss; // 卡尔曼增益 K Matrixdouble,15,3 K P * H.transpose() * (H * P * H.transpose() R).inverse(); // 误差状态增量 dx Matrixdouble,15,1 dx K * r; // 误差叠加到名义状态 ESKF核心⊕操作 // dp x_n.p dx.block3,1(0,0); // dv x_n.v dx.block3,1(3,0); // dtheta 姿态增量四元数左乘微小旋转 Vector3d dtheta dx.block3,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.block3,1(9,0); x_n.bg dx.block3,1(12,0); // Joseph形式更新协方差保证正定 Matrixdouble,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 IMUGNSS 15维测试 endl; // 模拟IMU预测100次 for(int i0;i100;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三、ESKFIMUGNSS优缺点优点误差状态是 15 维向量线性空间姿态使用 3 维切空间小量避免四元数 4 维冗余导致协方差奇异这是 ESKF 对比标准 EKF 最大优势。标准 EKF 直接估计四元数4 维姿态自由度冗余协方差容易发散。IMU 预测高频100~500HzGNSS 低频更新完美适配组合导航传感器异步特性。数值稳定性更好误差都是小量雅可比矩阵线性近似精度高每次更新后误差状态重置防止误差累积。偏置在线估计加速度计、陀螺仪 bias 实时估计不需要提前精细标定。工程落地广泛VIO、LIO、车载组合导航GNSSIMU主流方案GTSAM、VINS-Mono 都采用 ESKF 思想。缺点属于卡尔曼框架强依赖高斯噪声假设GNSS 出现粗差多路径、跳变时没有鲁棒性容易滤波发散。工程上需要额外增加异常检测残差卡方检验。仍然是一阶线性近似运动剧烈、大角度旋转时线性误差变大。IMU 积分会随时间漂移必须依赖外部观测GNSS持续修正长时间无 GNSS 信号隧道、室内定位漂移快速增长。需要仔细调参噪声矩阵(Q,R)对结果影响极大IMU 噪声参数需要实际标定凭经验设置会效果很差。实现细节坑多四元数归一化、协方差矩阵强制对称、姿态增量左乘 / 右乘坐标系 NED/ENU 容易搞混。四、代码说明 工程扩展提示坐标系当前代码使用NED北东地如果你需要 ENU重力向量改为Vector3d g(0,0,-9.81)姿态矩阵部分对应修改。噪声参数sigma_a sigma_g sigma_ba sigma_bg需要通过 IMU Allan 方差标定得到。GNSS 观测当前只使用位置观测如果需要 GNSS 速度观测可以扩展观测雅可比 H 矩阵增加速度残差。鲁棒改进增加残差卡方检测剔除 GNSS 野值可以切换 UKF 或者加入滑动窗口因子图优化GTSAM。扩展可以直接增加激光 / 视觉观测只需要新增观测方程和观测雅可比 H。
