多传感器融合定位：基于滤波的融合方法

点击上方“小白学视觉”，选择加"星标"或“置顶”

重磅干货，第一时间送达

SLAM 后端的优化方式大体分为滤波和优化。近些年优化越来越成为主流，在学习优化之前，掌握滤波的工作原理也十分必要。

一、引言

滤波问题可以简单理解为“预测 +观测 =融合结果”。

结合实际点云地图中定位的例子，预测由IMU给出，观测即为激光雷达点云和地图匹配得到的姿态和位置。

融合流程用框图可以表示如下

简述kalman滤波：

为了避免复杂的公式推导，大多数只给出结论：

贝叶斯滤波

贝叶斯滤波的信息流图如下：

在高斯假设前提下，用贝叶斯滤波的原始形式推导比较复杂，可以利用高斯的特征得到简化形式，即广义高斯滤波。后面KF、EKF、IEKF的推导均采用这种形式。

卡尔曼滤波(KF)推导

二、基于滤波的融合

1.状态方程

2.观测方程

3.构建滤波器

4.Kalman滤波实际使用流程

代码：

/*** @brief  init filter* @param  pose, init pose* @param  vel, init vel* @param  imu_data, init IMU measurements* @return true if success false otherwise*/
void ErrorStateKalmanFilter::Init(const Eigen::Vector3d &vel,const IMUData &imu_data) {// init odometry:Eigen::Matrix3d C_nb = imu_data.GetOrientationMatrix().cast<double>();// a. init C_nb using IMU estimation:pose_.block<3, 3>(0, 0) = C_nb;// b. convert flu velocity into navigation frame:vel_ = C_nb * vel;// save init pose:init_pose_ = pose_;// init IMU data buffer:imu_data_buff_.clear();imu_data_buff_.push_back(imu_data);// init filter time:time_ = imu_data.time;// set process equation in case of one step prediction & correction:Eigen::Vector3d linear_acc_init(imu_data.linear_acceleration.x,imu_data.linear_acceleration.y,imu_data.linear_acceleration.z);Eigen::Vector3d angular_vel_init(imu_data.angular_velocity.x,imu_data.angular_velocity.y,imu_data.angular_velocity.z);// covert to navigation frame:linear_acc_init =  linear_acc_init - accl_bias_;            //  body 系下angular_vel_init = GetUnbiasedAngularVel(angular_vel_init, C_nb);// init process equation, in case of direct correct step:UpdateProcessEquation(linear_acc_init, angular_vel_init);LOG(INFO) << std::endl<< "Kalman Filter Inited at " << static_cast<int>(time_)<< std::endl<< "Init Position: " << pose_(0, 3) << ", " << pose_(1, 3) << ", "<< pose_(2, 3) << std::endl<< "Init Velocity: " << vel_.x() << ", " << vel_.y() << ", "<< vel_.z() << std::endl;
}// 设置状态方程
void ErrorStateKalmanFilter::UpdateProcessEquation(const Eigen::Vector3d &linear_acc_mid,const Eigen::Vector3d &angular_vel_mid) {// set linearization point:Eigen::Matrix3d C_nb = pose_.block<3, 3>(0, 0);           //   n2b   转换矩阵Eigen::Vector3d f_b = linear_acc_mid + g_;                     //   加速度Eigen::Vector3d w_b = angular_vel_mid;                         //   角速度// set process equation:SetProcessEquation(C_nb, f_b, w_b);
}/*** @brief  set process equation* @param  C_nb, rotation matrix, body frame -> navigation frame* @param  f_n, accel measurement in navigation frame* @return void*/
void ErrorStateKalmanFilter::SetProcessEquation(const Eigen::Matrix3d &C_nb,                      //   更新状态方程  F矩阵const Eigen::Vector3d &f_b,const Eigen::Vector3d &w_b) {// TODO: set process / system equation:// a. set process equation for delta vel:F_.setZero();F_.block<3,  3>(kIndexErrorPos,  kIndexErrorVel)  =  Eigen::Matrix3d::Identity();F_.block<3,  3>(kIndexErrorVel,   kIndexErrorOri)  =  - C_nb *  Sophus::SO3d::hat(f_b).matrix();F_.block<3,  3>(kIndexErrorVel,   kIndexErrorAccel) =  -C_nb;F_.block<3,  3>(kIndexErrorOri,   kIndexErrorOri) =   - Sophus::SO3d::hat(w_b).matrix();F_.block<3,  3>(kIndexErrorOri,   kIndexErrorGyro) =   - Eigen::Matrix3d::Identity();// b. set process equation for delta ori:B_.setZero();B_.block<3,  3>(kIndexErrorVel,  kIndexNoiseGyro)  =    C_nb;B_.block<3,  3>(kIndexErrorOri,  kIndexNoiseGyro)  =     Eigen::Matrix3d::Identity();if(COV.PROCESS.BIAS_FLAG){      //  判断是否考虑随机游走B_.block<3,  3>(kIndexErrorAccel,  kIndexNoiseBiasAccel)  =     Eigen::Matrix3d::Identity();B_.block<3,  3>(kIndexErrorGyro,  kIndexNoiseBiasGyro)    =     Eigen::Matrix3d::Identity();}
}

// c. process noise:Q_.block<3, 3>(kIndexNoiseAccel, kIndexNoiseAccel) = COV.PROCESS.ACCEL * Eigen::Matrix3d::Identity();Q_.block<3, 3>(kIndexNoiseGyro, kIndexNoiseGyro) = COV.PROCESS.GYRO * Eigen::Matrix3d::Identity();if (COV.PROCESS.BIAS_FLAG ){Q_.block<3, 3>(kIndexNoiseBiasAccel, kIndexNoiseBiasAccel) = COV.PROCESS.BIAS_ACCEL * Eigen::Matrix3d::Identity();Q_.block<3, 3>(kIndexNoiseBiasGyro, kIndexNoiseBiasGyro) = COV.PROCESS.BIAS_GYRO * Eigen::Matrix3d::Identity();}// d. measurement noise:RPose_.block<3, 3>(0, 0) = COV.MEASUREMENT.POSE.POSI * Eigen::Matrix3d::Identity();RPose_.block<3, 3>(3, 3) = COV.MEASUREMENT.POSE.ORI * Eigen::Matrix3d::Identity();// e. process equation:F_.block<3, 3>(kIndexErrorPos, kIndexErrorVel) = Eigen::Matrix3d::Identity();F_.block<3, 3>(kIndexErrorOri, kIndexErrorGyro) = -Eigen::Matrix3d::Identity();B_.block<3, 3>(kIndexErrorOri, kIndexNoiseGyro) = Eigen::Matrix3d::Identity();B_.block<3, 3>(kIndexErrorAccel, kIndexNoiseBiasAccel) = Eigen::Matrix3d::Identity();B_.block<3, 3>(kIndexErrorGyro, kIndexNoiseBiasGyro) = Eigen::Matrix3d::Identity();// f. measurement equation:GPose_.block<3, 3>(0, kIndexErrorPos) = Eigen::Matrix3d::Identity();GPose_.block<3, 3>(3, kIndexErrorOri) = Eigen::Matrix3d::Identity();CPose_.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();CPose_.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity();// init soms:QPose_.block<kDimMeasurementPose, kDimState>(0, 0) = GPose_;

这部分是惯导解算的内容，在Updata()函数中：

在Updata()函数中有两个重要的函数，即UpdateOdomEstimation()，UpdateErrorEstimation()，分别做名义值更新和误差值更新

bool ErrorStateKalmanFilter::Update(const IMUData &imu_data) {//// TODO: understand ESKF update workflow//// update IMU buff:if (time_ < imu_data.time) {// update IMU odometry:Eigen::Vector3d linear_acc_mid;Eigen::Vector3d angular_vel_mid;imu_data_buff_.push_back(imu_data);UpdateOdomEstimation(linear_acc_mid, angular_vel_mid);  // 做名义值更新imu_data_buff_.pop_front();// update error estimation:double T = imu_data.time - time_;UpdateErrorEstimation(T, linear_acc_mid, angular_vel_mid);  // 做误差值更新// move forward:time_ = imu_data.time;return true;}return false;
}

名义值状态量（位置、速度、姿态、陀螺仪bias、加计bias）更新函数：UpdateOdomEstimation(linear_acc_mid, angular_vel_mid)：

void ErrorStateKalmanFilter::UpdateOdomEstimation(                  //  更新名义值 Eigen::Vector3d &linear_acc_mid, Eigen::Vector3d &angular_vel_mid) {//// TODO: this is one possible solution to previous chapter, IMU Navigation,// assignment//// get deltas:size_t   index_curr_  = 1;size_t   index_prev_ = 0;Eigen::Vector3d  angular_delta = Eigen::Vector3d::Zero();            GetAngularDelta(index_curr_,  index_prev_,   angular_delta,  angular_vel_mid);           //   获取等效旋转矢量,   保存角速度中值// update orientation:Eigen::Matrix3d  R_curr_  =  Eigen::Matrix3d::Identity();Eigen::Matrix3d  R_prev_ =  Eigen::Matrix3d::Identity();UpdateOrientation(angular_delta, R_curr_, R_prev_);                         //     更新四元数// get velocity delta:double   delta_t_;Eigen::Vector3d  velocity_delta_;GetVelocityDelta(index_curr_, index_prev_,  R_curr_,  R_prev_, delta_t_,  velocity_delta_,  linear_acc_mid);             //  获取速度差值， 保存线加速度中值// save mid-value unbiased linear acc for error-state update:// update position:UpdatePosition(delta_t_,  velocity_delta_);
}

惯导解算中，分别对应

//旋转向量解算：
bool ErrorStateKalmanFilter::GetAngularDelta(const size_t index_curr,const size_t index_prev,Eigen::Vector3d &angular_delta,Eigen::Vector3d &angular_vel_mid) {if (index_curr <= index_prev || imu_data_buff_.size() <= index_curr) {return false;}const IMUData &imu_data_curr = imu_data_buff_.at(index_curr);const IMUData &imu_data_prev = imu_data_buff_.at(index_prev);double delta_t = imu_data_curr.time - imu_data_prev.time;Eigen::Vector3d angular_vel_curr = Eigen::Vector3d(imu_data_curr.angular_velocity.x, imu_data_curr.angular_velocity.y,imu_data_curr.angular_velocity.z);Eigen::Matrix3d R_curr = imu_data_curr.GetOrientationMatrix().cast<double>();angular_vel_curr = GetUnbiasedAngularVel(angular_vel_curr, R_curr);Eigen::Vector3d angular_vel_prev = Eigen::Vector3d(imu_data_prev.angular_velocity.x, imu_data_prev.angular_velocity.y,imu_data_prev.angular_velocity.z);Eigen::Matrix3d R_prev = imu_data_prev.GetOrientationMatrix().cast<double>();angular_vel_prev = GetUnbiasedAngularVel(angular_vel_prev, R_prev);angular_delta = 0.5 * delta_t * (angular_vel_curr + angular_vel_prev);angular_vel_mid = 0.5 * (angular_vel_curr + angular_vel_prev);return true;
}// 姿态解算
void ErrorStateKalmanFilter::UpdateOrientation(const Eigen::Vector3d &angular_delta, Eigen::Matrix3d &R_curr,Eigen::Matrix3d &R_prev) {// magnitude:double angular_delta_mag = angular_delta.norm();// direction:Eigen::Vector3d angular_delta_dir = angular_delta.normalized();// build delta q:double angular_delta_cos = cos(angular_delta_mag / 2.0);double angular_delta_sin = sin(angular_delta_mag / 2.0);Eigen::Quaterniond dq(angular_delta_cos,angular_delta_sin * angular_delta_dir.x(),angular_delta_sin * angular_delta_dir.y(),angular_delta_sin * angular_delta_dir.z());Eigen::Quaterniond q(pose_.block<3, 3>(0, 0));// update:q = q * dq;// write back:R_prev = pose_.block<3, 3>(0, 0);pose_.block<3, 3>(0, 0) = q.normalized().toRotationMatrix();R_curr = pose_.block<3, 3>(0, 0);
}//速度解算
bool ErrorStateKalmanFilter::GetVelocityDelta(const size_t index_curr, const size_t index_prev,const Eigen::Matrix3d &R_curr, const Eigen::Matrix3d &R_prev, double &T,Eigen::Vector3d &velocity_delta, Eigen::Vector3d &linear_acc_mid) {if (index_curr <= index_prev || imu_data_buff_.size() <= index_curr) {return false;}const IMUData &imu_data_curr = imu_data_buff_.at(index_curr);const IMUData &imu_data_prev = imu_data_buff_.at(index_prev);T = imu_data_curr.time - imu_data_prev.time;Eigen::Vector3d linear_acc_curr = Eigen::Vector3d(imu_data_curr.linear_acceleration.x, imu_data_curr.linear_acceleration.y,imu_data_curr.linear_acceleration.z);Eigen::Vector3d  a_curr = GetUnbiasedLinearAcc(linear_acc_curr, R_curr);        //  w系下的a_currEigen::Vector3d linear_acc_prev = Eigen::Vector3d(imu_data_prev.linear_acceleration.x, imu_data_prev.linear_acceleration.y,imu_data_prev.linear_acceleration.z);Eigen::Vector3d  a_prev = GetUnbiasedLinearAcc(linear_acc_prev, R_prev);        //  w系下的a_prev// mid-value acc can improve error state prediction accuracy:linear_acc_mid = 0.5 * (a_curr + a_prev);     //  w 系下的linear_acc_mid , 用于更新pos_w 和 vel_wvelocity_delta = T * linear_acc_mid;linear_acc_mid = 0.5 * (linear_acc_curr + linear_acc_prev) - accl_bias_;      //  b 系下的linear_acc_midreturn true;
}// 位置解算
void ErrorStateKalmanFilter::UpdatePosition(const double &T, const Eigen::Vector3d &velocity_delta) {pose_.block<3, 1>(0, 3) += T * vel_ + 0.5 * T * velocity_delta;vel_ += velocity_delta;
}

kalman预测更新，误差值更新

void ErrorStateKalmanFilter::UpdateErrorEstimation(                       //  更新误差值const double &T, const Eigen::Vector3d &linear_acc_mid,const Eigen::Vector3d &angular_vel_mid) {static MatrixF F_1st;static MatrixF F_2nd;// TODO: update process equation:         //  更新状态方程UpdateProcessEquation(linear_acc_mid ,  angular_vel_mid);// TODO: get discretized process equations:         //   非线性化F_1st  =  F_ *  T;        //  T kalman 周期MatrixF   F = MatrixF::Identity()  +   F_1st;MatrixB  B =  MatrixB::Zero();B.block<3,  3>(kIndexErrorVel,  kIndexNoiseGyro)  =      B_.block<3,  3>(kIndexErrorVel,  kIndexNoiseGyro) * T;B.block<3,  3>(kIndexErrorOri,  kIndexNoiseGyro)  =      B_.block<3,  3>(kIndexErrorOri,  kIndexNoiseGyro) *T;if(COV.PROCESS.BIAS_FLAG){B.block<3,  3>(kIndexErrorAccel,  kIndexNoiseBiasAccel)  =    B_.block<3,  3>(kIndexErrorAccel,  kIndexNoiseBiasAccel)* sqrt(T);B.block<3,  3>(kIndexErrorGyro,  kIndexNoiseBiasGyro)  =      B_.block<3,  3>(kIndexErrorGyro,  kIndexNoiseBiasGyro)* sqrt(T);}// TODO: perform Kalman predictionX_ =  F * X_;P_ =  F * P_ * F.transpose()   +  B * Q_ * B.transpose();             //   只有方差进行了计算
}

根据是否有观测，来更新后验估计

void ErrorStateKalmanFilter::CorrectErrorEstimation(const MeasurementType &measurement_type, const Measurement &measurement) {//// TODO: understand ESKF correct workflow//Eigen::VectorXd Y;Eigen::MatrixXd G, K;switch (measurement_type) {case MeasurementType::POSE:CorrectErrorEstimationPose(measurement.T_nb, Y, G, K);break;default:break;}// TODO: perform Kalman correct:P_ = (MatrixP::Identity() -  K*G)  *  P_ ;          //  后验方差X_ =  X_ +  K * (Y - G*X_);                        //  更新后的状态量
}

当有观测时：依据下面公式

代码对应

/*** @brief  correct error estimation using pose measurement* @param  T_nb, input pose measurement* @return void*/
void ErrorStateKalmanFilter::CorrectErrorEstimationPose(                    //  计算  Y ，G  ，Kconst Eigen::Matrix4d &T_nb, Eigen::VectorXd &Y, Eigen::MatrixXd &G,Eigen::MatrixXd &K) {//// TODO: set measurement:      计算观测 delta pos  、 delta  ori//Eigen::Vector3d  dp  =  pose_.block<3,  1>(0,  3)  -   T_nb.block<3,  1>(0,  3);Eigen::Matrix3d  dR  =  T_nb.block<3,  3>(0, 0).transpose() *  pose_.block<3, 3>(0, 0) ;// TODO: set measurement equation:Eigen::Vector3d  dtheta  =  Sophus::SO3d::vee(dR  -  Eigen::Matrix3d::Identity() );YPose_.block<3, 1>(0, 0)  =  dp;          //    delta  position YPose_.block<3, 1>(3, 0)  =  dtheta;          //   失准角Y = YPose_;//   set measurement  GGPose_.setZero();GPose_.block<3, 3>(0, kIndexErrorPos)  =  Eigen::Matrix3d::Identity();GPose_.block<3 ,3>(3, kIndexErrorOri)   =  Eigen::Matrix3d::Identity();        G  =   GPose_;     //   set measurement  CCPose_.setZero();CPose_.block<3, 3>(0,kIndexNoiseAccel)   =  Eigen::Matrix3d::Identity();CPose_.block<3, 3>(3,kIndexNoiseGyro)    =  Eigen::Matrix3d::Identity();Eigen::MatrixXd  C  =   CPose_;// TODO: set Kalman gain:Eigen::MatrixXd R = RPose_;    //  观测噪声K =  P_  *  G.transpose() * ( G  *  P_  *  G.transpose( )  +  C * RPose_*  C.transpose() ).inverse() ;
}

对应代码

void ErrorStateKalmanFilter::EliminateError(void) {//      误差状态量  X_  :   15*1// TODO: correct state estimation using the state of ESKF//// a. position:// do it!pose_.block<3, 1>(0, 3)  -=  X_.block<3, 1>(kIndexErrorPos, 0 );   //  减去error// b. velocity:// do it!vel_ -=  X_.block<3,1>(kIndexErrorVel, 0 );         // c. orientation:// do it!Eigen::Matrix3d   dtheta_cross =  Sophus::SO3d::hat(X_.block<3,1>(kIndexErrorOri, 0));         //   失准角的反对称矩阵pose_.block<3, 3>(0, 0) =  pose_.block<3, 3>(0, 0) * (Eigen::Matrix3d::Identity() - dtheta_cross);     Eigen::Quaterniond  q_tmp(pose_.block<3, 3>(0, 0) );q_tmp.normalize();        //  为了保证旋转矩阵是正定的pose_.block<3, 3>(0, 0) = q_tmp.toRotationMatrix();  // d. gyro bias:if (IsCovStable(kIndexErrorGyro)) {gyro_bias_ -= X_.block<3, 1>(kIndexErrorGyro, 0);           //  判断gyro_bias_error是否可观}// e. accel bias:if (IsCovStable(kIndexErrorAccel)) {accl_bias_ -= X_.block<3, 1>(kIndexErrorAccel, 0);          //   判断accel_bias_error是否可观 }
}

void ErrorStateKalmanFilter::ResetState(void) {// reset current state:X_ = VectorX::Zero();
}

文章仅用于学术分享，如有侵权请联系删除

好消息！

小白学视觉知识星球

开始面向外开放啦