基于扩展卡尔曼滤波的多传感器融合定位
多传感器融合是一个复杂的问题，本文根据ROS中的robot localization功能包，说明多传感器融合定位的一种实现方式。
卡尔曼滤波
卡尔曼滤波并不复杂，分为预测和更新两个部分：
预测：
X^k1=AX^k−1+BukPk1=APk−1AT+Q\hat{X}_{k}^{1} =A\hat{X}_{k-1}+Bu_{k}\\ P_{k}^{1}=AP_{k-1}A^{T}+Q X^k1=AX^k−1+BukPk1=APk−1AT+Q
更新：
Kk=Pk1HTHPk1HT+RX^k=X^k1+Kk(Zk−HX^k1)Pk=(I−KkH)Pk1K_{k}=\frac{P_{k}^{1}H^{T}}{HP_{k}^{1}H^{T}+R}\\ \hat{X}_{k}=\hat{X}_{k}^{1}+K_{k}(Z_{k}-H \hat{X}_{k}^{1})\\ P_{k}=(I-K_{k}H)P_{k}^{1} Kk=HPk1HT+RPk1HTX^k=X^k1+Kk(Zk−HX^k1)Pk=(I−KkH)Pk1
在实际使用卡尔曼滤波时，第一是确定状态变量和控制变量，一般在机器人定位中，8个状态变量 [x,y,θ,vx,vy,ω,ax,ay][x,y,\theta,v_{x},v_{y},\omega,a_{x},a_{y}][x,y,θ,vx,vy,ω,ax,ay]都可以看成是状态变量，也可以把加速度看成是控制变量，得到加速度的方法可以通过上一篇文章讲的动力学控制中得到，也可以通过速度做差分。如果是都看成状态变量，那么加速度的更新是通过更新步的第二个式子进行更新，预测步的第一个式子是无法更新加速度的，只会保留加速度。
第二个问题是，状态变量的选取，一般来说定位是机器人在地图上的定位，使用的是世界坐标系，即 [x,y,θ][x,y,\theta][x,y,θ]是世界坐标系下的变量，而速度、角速度、加速度等都是通过传感器计算得到的，是机器人坐标系下的变量。从机器人坐标系转到世界坐标系需要通过旋转变换，即会引入三角函数，这样状态方程就是非线性的了，因此需要使用扩展卡尔曼滤波，这时公式需要做修改，AAA矩阵为雅可比矩阵
xk1=f(x^k−1,uk)z=h(xk)H=∂h∂xk∣x^k1,A=∂f∂xk−1∣x^k−11x_{k}^{1}=f(\hat{x}_{k-1},u_{k})\\ z=h(x_{k})\\ H=\frac{\partial h}{\partial x_{k}}|_{\hat{x}_{k}^{1} },A=\frac{\partial f}{\partial x_{k-1}}|_{\hat{x}_{k-1}^{1} } xk1=f(x^k−1,uk)z=h(xk)H=∂xk∂h∣x^k1,A=∂xk−1∂f∣x^k−11例如：
第三个问题是 P0,Q,RP_{0},Q,RP0,Q,R矩阵应该如何选取。首先是RRR矩阵，测量噪声协方差矩阵，一般是传感器的测量噪声，常用的传感器有IMU，激光雷达，摄像头，编码器等，网上都有常用的噪声协方差矩阵，这个不难找。然后是P0P_{0}P0和QQQ，初始的先验误差协方差矩阵和过程噪声矩阵，这个可以参考一些论文或开源的ekf包，里边有。本文参考的robot localization中，这两个矩阵为：
这里的矩阵是15维的：[x,y,z,roll,pitch,yaw,vx,vy,vz,θroll,θpitch,θyaw,ax,ay,az][x,y,z,roll, pitch, yaw, v_{x}, v_{y}, v_{z}, \theta_{roll}, \theta_{pitch}, \theta_{yaw}, a_{x}, a_{y}, a_{z}][x,y,z,roll,pitch,yaw,vx,vy,vz,θroll,θpitch,θyaw,ax,ay,az]
多传感器融合
多传感器融合是一个复杂的方向，具体可以分为紧耦合和松耦合两种融合方式。紧耦合是将各传感器的数据汇总，然后计算出一个位姿，松耦合是各传感器的数据分别计算出一个位姿，然后再进行汇总得到最终位姿。这么说好像有点不太直观，总之紧耦合的特点是各传感器的数据需要同步，而松耦合的传感器是异步的，不需要同步。robot localization功能包中的EKF是松耦合的融合方式。
多传感器融合的第一个问题是数据处理，由于robot localization包是松耦合的方式，因此对每一个传感器数据单独进行处理。ROS中的协方差矩阵分为三类，位姿、速度和加速度，因此所有传感器的原始数据也会拆分成这三类。举例平面机器人如下：
编码器里程计可以计算位置和速度，使用[θ,vx,vy][\theta,v_{x},v_{y}][θ,vx,vy]数据进行融合，首先先对一个传感器的数据进行分类，θ\thetaθ是一组，vx,vyv_{x},v_{y}vx,vy是一组，前者是pose类型的数据，后者是twist类型的数据，处理pose时，将角度值和其协方差放入一个结构体中，作为其中一组测量数据；处理twist时，将两个速度和对应的协方差矩阵放入另一个结构体中，作为另一组测量数据。
数据处理完后进行卡尔曼滤波，预测步没啥好说的，在更新步中，由于第一组测量数据只有角度值，因此进行更新时只更新角度值，即H矩阵只在角度对应的行，斜对角线上的值为1，其余为0。更新完角度后，协方差矩阵P更新，然后不进行预测了，因为都是同一个时间戳的数据，继续更新速度，H矩阵的值改一下，进行第二次更新。
robot localization功能包运行原理
该功能包分为主函数和传感器数据接收回调函数两个线程。
主函数部分：首先是ekf_localization_node.cpp下的main函数：看代码没啥好说的吧，然后进入这个ekf.run()函数。
int main(int argc, char **argv){ros::init(argc, argv, "ekf_navigation_node"); //创建节点RobotLocalization::RosEkf ekf;//类ekf.run();//进入这个函数return EXIT_SUCCESS;}
ekf.run()这个函数在ros_filter.cpp下1742行（行数可能不一样，因为我的robot localization包原代码被我修改过，但大概就是上下几行）。这个函数中第一个重要的部分就是，这个是读取参数，具体读了什么，可以进去看看，主要是你ekf_params.yaml这里设置的内容。注意这里加载了参数后，回调函数就开始起作用了，具体的内容放后边说。
loadParams();
然后下边就是发布者，tf变换啥的，tf变换是把测量数据从一个坐标系变换到另一个坐标系，还有打印一些东西，主要是让你知道的程序为什么报错的，我们直接忽略，接下来是时间。时间的问题在该包中十分重要，这里是获取当前时间戳，ROS的传感器消息都自带时间戳，储存在header中。
ros::Time curTime;ros::Time lastDiagTime = ros::Time::now();
以上是初始化的一部分内容，然后就进入死循环进行不断的更新和预测了。接下来先说说回调函数的内容。
回调函数，说的是接收到传感器数据后进入回调处理的过程。具体的处理方式函数在ros_filter.h 105行的类中有详细的说明，这里列一部分比较有用的。
template<class T> class RosFilter{/*控制量回调：这部分就是前文讲的，将加速度看成是控制量还是状态变量，如果是看成控制量，那就会根据速度差分的方法计算加速度，同时也会加载最大最小加速度限制、加加速度限制的相关参数，该包默认是不使用控制量。*/void controlCallback(const geometry_msgs::Twist::ConstPtr &msg);void controlCallback(const geometry_msgs::TwistStamped::ConstPtr &msg);/*三种类型数据的回调处理，这里和前文说的差不多，但是除了测量值和协方差矩阵外，还加上了很多东西，比如时间戳等等*/void accelerationCallback(const sensor_msgs::Imu::ConstPtr &msg,const CallbackData &callbackData,const std::string &targetFrame);void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg,const CallbackData &callbackData,const std::string &targetFrame,const bool imuData);void twistCallback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg,const CallbackData &callbackData,const std::string &targetFrame);/*IMU数据和里程计数据回调函数*/void imuCallback(const sensor_msgs::Imu::ConstPtr &msg, const std::string &topicName,const CallbackData &poseCallbackData, const CallbackData &twistCallbackData,const CallbackData &accelCallbackData);void odometryCallback(const nav_msgs::Odometry::ConstPtr &msg, const std::string &topicName,const CallbackData &poseCallbackData, const CallbackData &twistCallbackData);}
进入odometryCallback()，
/*根据时间戳检查是否是过去的测量数据，每次进行预测更新后，lastSetPoseTime_时间戳都会变成使用的测量值的时间戳*/if (msg->header.stamp <= lastSetPoseTime_){···}/*数据分类处理了，先处理pose，再处理twist*/if (poseCallbackData.updateSum_ > 0){// Grab the pose portion of the message and pass it to the poseCallbackgeometry_msgs::PoseWithCovarianceStamped *posPtr = new geometry_msgs::PoseWithCovarianceStamped();posPtr->header = msg->header;posPtr->pose = msg->pose; // Entire pose object, also copies covariancegeometry_msgs::PoseWithCovarianceStampedConstPtr pptr(posPtr);poseCallback(pptr, poseCallbackData, worldFrameId_, false);}if (twistCallbackData.updateSum_ > 0){// Grab the twist portion of the message and pass it to the twistCallbackgeometry_msgs::TwistWithCovarianceStamped *twistPtr = new geometry_msgs::TwistWithCovarianceStamped();twistPtr->header = msg->header;twistPtr->header.frame_id = msg->child_frame_id;twistPtr->twist = msg->twist; // Entire twist object, also copies covariancegeometry_msgs::TwistWithCovarianceStampedConstPtr tptr(twistPtr);twistCallback(tptr, twistCallbackData, baseLinkFrameId_);}
进入poseCallback()，
if (msg->header.stamp >= lastMessageTimes_[topicName]){// 创建向量和矩阵，向量用来存放测量值，矩阵存放协方差Eigen::VectorXd measurement(STATE_SIZE);Eigen::MatrixXd measurementCovariance(STATE_SIZE, STATE_SIZE);measurement.setZero();measurementCovariance.setZero();···//这个if就是进行处理了，具体处理些个啥，有兴趣可以点进去看，if (preparePose(msg,topicName,targetFrame,callbackData.differential_,callbackData.relative_,imuData,updateVectorCorrected,measurement,measurementCovariance)){/*这个函数就是把处理好的数据，measurement这个放入测量数据队列中这样每一个传感器数据就被分解成pose、twist、acc等三类数据存到队列中*/enqueueMeasurement(topicName,measurement,measurementCovariance,updateVectorCorrected,callbackData.rejectionThreshold_,msg->header.stamp);}···}
这样我们的传感器数据经过这样的回调处理后，最后得到一个队列：[imu_pose,imu_twist,odom1_pose,odom1_twist,odom2_pose…]
说回主函数，进入到死循环后，主要有以下几个函数起作用，
while (ros::ok()){// 获取当前时间戳curTime = ros::Time::now();// 将队列里的传感器数据取出，ekf就运行在这个函数中integrateMeasurements(curTime);// Get latest state and publish itnav_msgs::Odometry filteredPosition;···//后面就是一些尾巴的处理，什么坐标系变换啊，发布话题啊，数据重新跟新啊啥的。}
进入integrateMeasurements(curTime);
while (!measurementQueue_.empty() && ros::ok()){//检查队列是否为空，检查队头的数据时间是不是过去的，MeasurementPtr measurement = measurementQueue_.top();if (measurement->time_ > currentTime.toSec()){break;}measurementQueue_.pop();if (useControl_ && restoredMeasurementCount > 0){filter_.setControl(measurement->latestControl_, measurement->latestControlTime_);restoredMeasurementCount--;}// EKF的预测和更新步filter_.processMeasurement(*(measurement.get()));···}
进入filter_.processMeasurement();
if (initialized_){// 时间差🔺tdelta = measurement.time_ - lastMeasurementTime_;if (delta > 0){validateDelta(delta);// 预测步predict(measurement.time_, delta);// Return this to the userpredictedState_ = state_;}// 更新步correct(measurement);}
predict(measurement.time_, delta);在ekf.cpp中
···//前边都是雅可比矩阵的计算// (1) Apply control terms, which are actually accelerations，controlAcceleration_都是0，因为无控制量state_(StateMemberVroll) += controlAcceleration_(ControlMemberVroll) * delta;state_(StateMemberVpitch) += controlAcceleration_(ControlMemberVpitch) * delta;state_(StateMemberVyaw) += controlAcceleration_(ControlMemberVyaw) * delta;//这个加速度也不变，和上一次状态变量的值一样，第(1)步几乎没用state_(StateMemberAx) = (controlUpdateVector_[ControlMemberVx] ?controlAcceleration_(ControlMemberVx) : state_(StateMemberAx));state_(StateMemberAy) = (controlUpdateVector_[ControlMemberVy] ?controlAcceleration_(ControlMemberVy) : state_(StateMemberAy));state_(StateMemberAz) = (controlUpdateVector_[ControlMemberVz] ?controlAcceleration_(ControlMemberVz) : state_(StateMemberAz));// (2) Project the state forward: x = Ax + Bu (really, x = f(x, u))，预测步第一个式子state_ = transferFunction_ * state_;// Handle wrappingwrapStateAngles();// (3) Project the error forward: P = J * P * J' + Q，第二个式子estimateErrorCovariance_ = (transferFunctionJacobian_ *estimateErrorCovariance_ *transferFunctionJacobian_.transpose());estimateErrorCovariance_.noalias() += delta * (*processNoiseCovariance);}
correct();也在ekf.cpp中
···// 各个矩阵Eigen::VectorXd stateSubset(updateSize);// x (in most literature)Eigen::VectorXd measurementSubset(updateSize); // zEigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize); // REigen::MatrixXd stateToMeasurementSubset(updateSize, state_.rows()); // HEigen::MatrixXd kalmanGainSubset(state_.rows(), updateSize);// KEigen::VectorXd innovationSubset(updateSize);// z - HxstateSubset.setZero();measurementSubset.setZero();measurementCovarianceSubset.setZero();stateToMeasurementSubset.setZero();kalmanGainSubset.setZero();innovationSubset.setZero();for (size_t i = 0; i < updateSize; ++i){// 这里就是改变测量矩阵H的值了，只在测量数据部分有值。measurementSubset(i) = measurement.measurement_(updateIndices[i]);···}···//后面更新就是套公式了
放两张更新时的图，分别是odometry的pose和twist数据进行更新的，state就是目前的状态，11是θ˙z\dot{\theta}_{z}θ˙z数据所在的位置，其它部分是0，是因为这是平面机器人。6是vxv_{x}vx测量数据所在的位置，机器人不是全向的，所有7处vyv_{y}vy总为0。
