ROS实验笔记之——基于Augmented State EKF的多传感器融合（IMU，PnP，VO）

最新推荐文章于 2022-04-09 21:45:18 发布
最新推荐文章于 2022-04-09 21:45:18 发布
阅读量1.1k 收藏 5
点赞数 1
分类专栏： ROS
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/gwplovekimi/article/details/116520996
版权
ROS 同时被 2 个专栏收录
147 篇文章 181 订阅
订阅专栏
移动机器人
82 篇文章 118 订阅
订阅专栏

在之前的博客中，分别实现了如下的实验：

本博文通过Augmented State EKF来实现多传感器的融合（实现效果好像有点问题，debug中。。。。。）

 

实验视频如下：

基于Augmented State EKF来实现多传感器的融合（IMU，PNP，VO）

 

目录

Augmented State EKF 基本原理的介绍

代码

测试

 

Augmented State EKF 基本原理的介绍

在有些情况下，只有relative state measurements。比如之前博客《ROS实验笔记之——基于stereo camera的视觉里程计的实现》介绍的基于关键帧的visual odometry。那么这个时候，测量值中就包含了关键帧的数据了。

而对应的state可以写为如下的形式：

对应的协方差矩阵为：

其中，关键帧对应的状态值称为augmented states（m个）。而对于augmented states的增加与减少，可见下图

通过关键帧的引入，来提供global localization，从而保证overall performance。这是由于有了关键帧来保证不会漂？

接下来举个例子。假如一个quadrotor具备Good Acceleration Sensorand Keyframe-based Visual Odometry

那么为了将其relztive pose的测量与singl-keyframe visual odometry进行融合，将state增强为：

对应的process model为（prediction step之会影响mian state不会影响Augmented State）：

对应的测量模型为：

而关键帧的改变，则需要对augmented state进行更新如下:

 

代码

关于TF（frame transformation），将所有的relative pose measurement转到body frame（对准IMU）；将global pose measurement （PnP）转到world frame。

接下来思考于注释都写在代码中～

ekf_math.h

#pragma once

#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <math.h>

//定义了一系列函数，类似于之前EKF的project一样，只是定义于头文件中，让整个工程整洁一些～

inline Eigen::Vector3d rotationMatrix2EulerVector_zxy(Eigen::Matrix3d R)
{
    Eigen::Vector3d xyz_vector;
    double phi = asinf64(R(2, 1));
    double theta = atan2f64(-R(2, 0), R(2, 2));
    double psi = atan2f64(-R(0, 1), R(1, 1));
    xyz_vector << phi, theta, psi;
    return xyz_vector;
}
    
inline Eigen::Matrix3d Rotation_matrix(double roll, double pitch, double yaw)
{
    Eigen::Matrix3d R;
    R = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) * 
        Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX()) * 
        Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY());
    return R;
}

inline Eigen::Matrix3d Ginv(double roll, double pitch, double yaw)
{
    Eigen::Matrix3d G_inv;
    // G << cos(pitch), 0, -cos(roll) * sin(pitch),
    //      0,          1, sin(roll),
    //      sin(pitch), 0, cos(roll) * cos(pitch);

    G_inv << cos(pitch),                        0,                      sin(pitch),
            (sin(roll)*sin(pitch))/cos(roll),   1, -(cos(pitch)*sin(roll))/cos(roll),
            -sin(pitch)/cos(roll),              0,             cos(pitch)/cos(roll);
    return G_inv;
}
inline Eigen::Matrix3d delGinv_delroll(double roll, double pitch)
{
    // \frac{\partial G}{\partial \phi}
    Eigen::Matrix3d deltaG;
    double theta = pitch;
    double phi = roll;
    double cos2_1 = 1.0 / (cos(phi) * cos(phi) + 1e-8);
    deltaG << 0, 0, 0, 
                sin(theta) * cos2_1, 0, -cos(theta) * cos2_1,
                -sin(phi) * sin(theta) * cos2_1, 0, cos(theta) * sin(phi) * cos(phi);
    return deltaG; 
}


inline Eigen::Matrix3d delGinv_delpitch(double roll, double pitch)
{
    // \frac{\partial G}{\partial \theta}
    Eigen::Matrix3d deltaG;
    double theta = pitch;
    double phi = roll;
    double cos_1 = 1.0 / (cos(phi) + 1e-8);
    deltaG << -sin(theta), 0, cos(theta), 
        cos(theta) * sin(phi) * cos_1, 0, sin(theta) * sin(phi) * cos_1,
        -cos(theta)* cos_1, 0 , - sin(theta) * cos_1;
    return deltaG;
}

inline Eigen::Matrix3d deltaR_deltaroll(double roll, double pitch, double yaw)
{
    // \frac{\partial R}{\partial \phi}
    Eigen::Matrix3d deltaR;
    double theta = pitch;
    double phi = roll;
    double psi = yaw;

    deltaR << 
        -cos(phi)*sin(psi)*sin(theta),  sin(phi)*sin(psi), cos(phi)*cos(theta)*sin(psi),
         cos(phi)*cos(psi)*sin(theta), -cos(psi)*sin(phi), -cos(phi)*cos(psi)*cos(theta),
         sin(phi)*sin(theta), cos(phi), -cos(theta)*sin(phi);
        
    return deltaR;
}

inline Eigen::Matrix3d deltaR_deltapitch(double roll, double pitch, double yaw)
{
    // \frac{\partial R}{\partial \phi}
    Eigen::Matrix3d deltaR;
    double theta = pitch;
    double phi = roll;
    double psi = yaw;

    deltaR << 
    - cos(psi)*sin(theta) - cos(theta)*sin(phi)*sin(psi), 0, cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta),
         cos(psi)*cos(theta)*sin(phi) - sin(psi)*sin(theta), 0,  cos(theta)*sin(psi) + cos(psi)*sin(theta)*sin(phi),
         -cos(phi)*cos(theta)                               , 0, -cos(phi)*sin(theta);
    return deltaR;
}

inline Eigen::Matrix3d deltaR_deltayaw(double roll, double pitch, double yaw)
{
    // \frac{\partial R}{\partial \phi}
    Eigen::Matrix3d deltaR;
    double theta = pitch;
    double phi = roll;
    double psi = yaw;

    deltaR << - cos(theta)*sin(psi) - cos(psi)*sin(phi)*sin(theta), -cos(phi)*cos(psi), cos(psi)*cos(theta)*sin(phi) - sin(psi)*sin(theta),
   cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta), -cos(phi)*sin(psi), cos(psi)*sin(theta) + sin(phi)*sin(psi)*cos(theta),
    0, 0, 0;
    return deltaR;
}

inline Eigen::Matrix3d deltatheta_deltaR(Eigen::Matrix3d R)
{
  Eigen::Matrix3d dtheta_dR = Eigen::MatrixXd::Zero(3, 3);
  dtheta_dR(2, 2) = R(2, 0) / (R(2, 0) * R(2, 0) + R(2, 2) * R(2, 2));
  dtheta_dR(2, 0) = R(2, 2) / (R(2, 0) * R(2, 0) + R(2, 2) * R(2, 2));
  return dtheta_dR;
}
inline Eigen::Matrix3d deltaphi_deltaR(Eigen::Matrix3d R)
{
  Eigen::Matrix3d dphi_dR = Eigen::MatrixXd::Zero(3, 3);
  dphi_dR(2, 1) = 1.0 / sqrt(1 - R(2, 1) * R(2, 1));
  return dphi_dR;
}
inline Eigen::Matrix3d deltayaw_deltaR(Eigen::Matrix3d R)
{
  Eigen::Matrix3d dyaw_dR = Eigen::MatrixXd::Zero(3, 3);
  dyaw_dR(1, 1) = R(0, 1) / (R(0, 1) * R(0, 1) + R(1, 1) *R(1, 1));
  dyaw_dR(0, 1) = R(1, 1) / (R(0, 1) * R(0, 1) + R(1, 1) *R(1, 1));
  return dyaw_dR;
}

ekf_node.cpp

#include <iostream>
#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>

#include <ekf_filter.h>

// for debug
// #include <backward.hpp>
// namespace backward {
// backward::SignalHandling sh;
// }

using namespace std;
using namespace Eigen;

int main(int argc, char** argv) {
  ros::init(argc, argv, "aug_ekf");
  ros::NodeHandle n("~");

  ekf_imu_vision::EKFImuVision aug_ekf;
  aug_ekf.init(n);//对类进行了初始化

  ros::spin();
}

ekf_filter.cpp

#include <ekf_filter.h>


namespace ekf_imu_vision {
EKFImuVision::EKFImuVision(/* args */) {}

EKFImuVision::~EKFImuVision() {}

void EKFImuVision::init(ros::NodeHandle& nh) {
  node_ = nh;

  /* ---------- parameter ---------- */
  Qt_.setZero();
  Rt1_.setZero();
  Rt2_.setZero();

  // addition and removel of augmented state
  
  // TODO
  // set M_a_ and M_r_
  M_r_.setZero();
  M_r_.block<15,15>(0,0) = Eigen::MatrixXd::Identity(15,15);

  M_a_.setZero();
  M_a_.block<15,15>(0,0) = Eigen::MatrixXd::Identity(15, 15);
  M_a_.block<6,6>(15,0) = Eigen::MatrixXd::Identity(6, 6);


  for (int i = 0; i < 3; i++) {
    /* process noise */
    node_.param("aug_ekf/ng", Qt_(i, i), -1.0);
    node_.param("aug_ekf/na", Qt_(i + 3, i + 3), -1.0);
    node_.param("aug_ekf/nbg", Qt_(i + 6, i + 6), -1.0);
    node_.param("aug_ekf/nba", Qt_(i + 9, i + 9), -1.0);
    node_.param("aug_ekf/pnp_p", Rt1_(i, i), -1.0);
    node_.param("aug_ekf/pnp_q", Rt1_(i + 3, i + 3), -1.0);
    node_.param("aug_ekf/vo_pos", Rt2_(i, i), -1.0);
    node_.param("aug_ekf/vo_rot", Rt2_(i + 3, i + 3), -1.0);
  }

  init_        = false;

  for(int i = 0; i < 4; i++)
    latest_idx[i] = 0;

  /* ---------- subscribe and publish ---------- */
  imu_sub_ =
      node_.subscribe<sensor_msgs::Imu>("/dji_sdk_1/dji_sdk/imu", 100, &EKFImuVision::imuCallback, this);
  pnp_sub_     = node_.subscribe<nav_msgs::Odometry>("tag_odom", 10, &EKFImuVision::PnPCallback, this);
  // opti_tf_sub_ = node_.subscribe<geometry_msgs::PointStamped>("opti_tf_odom", 10,
                                                              // &EKFImuVision::opticalCallback, this);
  stereo_sub_  = node_.subscribe<stereo_vo::relative_pose>("/vo/Relative_pose", 10,
                                                          &EKFImuVision::stereoVOCallback, this);
  fuse_odom_pub_ = node_.advertise<nav_msgs::Odometry>("ekf_fused_odom", 10);
  path_pub_         = node_.advertise<nav_msgs::Path>("/aug_ekf/Path", 100);

  ros::Duration(0.5).sleep();

  ROS_INFO("Start ekf.");
}

void EKFImuVision::rectMeasPnP(const nav_msgs::OdometryConstPtr& msg, Vec6 &ut){
  double rel_x, rel_y, rel_z, rel_qw, rel_qx, rel_qy, rel_qz;
  rel_x = msg->pose.pose.position.x;
  rel_y = msg->pose.pose.position.y;
  rel_z = msg->pose.pose.position.z;

  rel_qw = msg->pose.pose.orientation.w;
  rel_qx = msg->pose.pose.orientation.x;
  rel_qy = msg->pose.pose.orientation.y;
  rel_qz = msg->pose.pose.orientation.z;
  Eigen::Quaterniond q_kc(rel_qw, rel_qx, rel_qy, rel_qz);
  Mat3x3 R_cam_tag(q_kc);
  Eigen::Matrix4d T_w_tag, T_i_down, T_cam_tag, T_w_b;
  T_cam_tag.setZero();
  T_cam_tag.block<3,3>(0,0) = R_cam_tag;
  T_cam_tag(0,3) = rel_x;
  T_cam_tag(1,3) = rel_y;
  T_cam_tag(2,3) = rel_z;
  T_cam_tag(3,3) = 1;

  T_w_tag<< 0, 1, 0, 0,
          1, 0, 0 , 0,
          0 ,0, -1, 0,
          0, 0, 0, 1;
  T_i_down << 1, 0, 0, 0.07,
          0, -1, 0, -0.02,
          0, 0, -1, 0.01,
          0, 0, 0, 1;
  T_w_b = T_w_tag * T_cam_tag.inverse() * T_i_down.inverse();

  Vec3 rpy_wb = rotation2Euler(T_w_b.block<3,3>(0,0));

  Vec6 temp;

  temp<<T_w_b.block<3,1>(0,3), rpy_wb;
  ut = temp;

}

void EKFImuVision::PnPCallback(const nav_msgs::OdometryConstPtr& msg) {

  // TODO 
  // construct a new AugState using the absolute measurement from marker PnP and insert the new state into the queue
  ROS_INFO("Receiving pnp: %f", msg->header.stamp.toSec());

  AugState state0;
  state0.type = pnp;
  state0.time_stamp = msg->header.stamp;
  rectMeasPnP(msg, state0.ut);

  if(!init_){
    int insert_pos = -1, que_end = aug_state_hist_.size()-1;
    if(que_end<0 || state0.time_stamp < aug_state_hist_.front().time_stamp)
      insert_pos = 0;
    else {
      for(int i=que_end;i>=0;i--)
        if(aug_state_hist_.at(i).time_stamp<state0.time_stamp) {
          insert_pos = i+1;
          break;
        }
    }
    if(insert_pos<que_end+1)
      latest_idx[imu]++;
    aug_state_hist_.insert(aug_state_hist_.begin()+insert_pos, state0);
    aug_state_hist_.erase(aug_state_hist_.begin(),aug_state_hist_.begin()+insert_pos);
    latest_idx[imu]-=insert_pos;
    if(latest_idx[imu]<0)
      latest_idx[imu]++;

    initUsingPnP(aug_state_hist_.begin());
    latest_idx[pnp] = 0;
    init_ = true;
    return;
  }

  insertNewState(state0, false);

}

void EKFImuVision::stereoVOCallback(const stereo_vo::relative_poseConstPtr& msg) {

  // TODO
  // if the keyframe is changed, label the state associated with the new keyframe in the queue
  // construct a new AugState using the relative measurement from VO and insert the new state into the queue

  ROS_INFO("Receiving VO : %f, KF: %f", msg->header.stamp.toSec(), msg->key_stamp.toSec());
//  ROS_INFO("Receiving VO : %d, KF: %d", msg->header.stamp.nsec, msg->previous_stamp.nsec);

  if(!init_)
    return;
  if(msg->key_stamp<aug_state_hist_.front().time_stamp)
    return;

  ros::Time keyframe_time = msg->key_stamp, vo_time = msg->header.stamp;
  double rel_x, rel_y, rel_z, rel_qw, rel_qx, rel_qy, rel_qz;
  rel_x = msg->relative_pose.position.x;
  rel_y = msg->relative_pose.position.y;
  rel_z = msg->relative_pose.position.z;

  rel_qw = msg->relative_pose.orientation.w;
  rel_qx = msg->relative_pose.orientation.x;
  rel_qy = msg->relative_pose.orientation.y;
  rel_qz = msg->relative_pose.orientation.z;

  Eigen::Quaterniond q_kc(rel_qw, rel_qx, rel_qy, rel_qz);
  Mat3x3 R_kc(q_kc);
  Vec3 rpy = rotation2Euler(R_kc);

  AugState to_insert;
  to_insert.time_stamp = vo_time;
  to_insert.ut(0) = rel_x;
  to_insert.ut(1) = rel_y;
  to_insert.ut(2) = rel_z;
  to_insert.ut.segment(3,3) = rpy;
  to_insert.type = vo;
  to_insert.key_frame_time_stamp = keyframe_time;

  if(keyframe_time == aug_state_hist_.at(latest_idx[keyframe]).time_stamp)
    insertNewState(to_insert, false);
  else
    insertNewState(to_insert, true);


}

void EKFImuVision::imuCallback(const sensor_msgs::ImuConstPtr& imu_msg) {

  // TODO
  // construct a new AugState using the IMU input and insert the new state into the queue

//  ROS_INFO("Receiving IMU: %f", imu_msg->header.stamp.toSec());

  double wx, wy, wz, ax, ay, az;

  wx = imu_msg->angular_velocity.x;
  wy = imu_msg->angular_velocity.y;
  wz = imu_msg->angular_velocity.z;
  ax = imu_msg->linear_acceleration.x;
  ay = imu_msg->linear_acceleration.y;
  az = imu_msg->linear_acceleration.z;

  Vec6 ut;
  ut<<wx, wy, wz, ax, ay, az;

  AugState curr;
  curr.type = imu;
  curr.time_stamp = imu_msg->header.stamp;
  curr.ut = ut;
  aug_state_hist_.push_back(curr);
  latest_idx[imu]=aug_state_hist_.size()-1;

}

bool EKFImuVision::insertNewState(AugState& new_state, bool change_keyframe) {

  // TODO
  // insert the new AugState to the queue and then do repropagation to update the mean & covariance using the input/measurement. Call repropagate() here.

  int insert_pos = -1, que_end = aug_state_hist_.size()-1;
  if(que_end<0 || new_state.time_stamp < aug_state_hist_.front().time_stamp)
    insert_pos = 0;
  else {
    for(int i=que_end;i>=0;i--)
      if(aug_state_hist_.at(i).time_stamp<new_state.time_stamp) {
        insert_pos = i+1;
        break;
      }
  }

  for(unsigned int & i : latest_idx)
    if(i>=insert_pos)
      i++;
  aug_state_hist_.insert(aug_state_hist_.begin()+insert_pos, new_state);

  if(new_state.type == pnp){
    int prop_start = min(max(latest_idx[pnp], latest_idx[vo])+1, (unsigned int)insert_pos),
        prop_end = max(max(latest_idx[pnp], latest_idx[vo]), (unsigned int)insert_pos);
//    for (auto it = aug_state_hist_.begin() + prop_start; it != aug_state_hist_.begin() + prop_end + 1; it++) {
    for (int i = prop_start; i <= prop_end; i++) {
      if(aug_state_hist_.at(i).type == pnp){
        updatePnP(aug_state_hist_.at(i), aug_state_hist_.at(i-1));
      } else if(aug_state_hist_.at(i).type == imu){
        predictIMU(aug_state_hist_.at(i), aug_state_hist_.at(i-1));
      } else if(aug_state_hist_.at(i).type == vo){
        updateVO(aug_state_hist_.at(i),aug_state_hist_.at(i-1));
      } else{
        updateVO(aug_state_hist_.at(i), aug_state_hist_.at(i-1));
        changeAugmentedState(aug_state_hist_.at(i));
      }
    }

    latest_idx[pnp] = insert_pos;

  } else{
    if(change_keyframe){
      int newkf_pos = -1;
      for(int i=insert_pos;i>0;i--){
        if(aug_state_hist_.at(i-1).time_stamp <= new_state.time_stamp &&
          new_state.time_stamp <= aug_state_hist_.at(i).time_stamp){
          if(new_state.time_stamp - aug_state_hist_.at(i-1).time_stamp >
            aug_state_hist_.at(i).time_stamp - new_state.time_stamp){
            newkf_pos = i;
          } else newkf_pos = i-1;
          break;
        }
      }

      aug_state_hist_.at(insert_pos).key_frame_time_stamp = aug_state_hist_.at(newkf_pos).time_stamp;
      int prop_start = min(max(latest_idx[pnp], latest_idx[vo])+1, (unsigned int)newkf_pos),
              prop_end = max(max(latest_idx[pnp], latest_idx[vo]), (unsigned int)insert_pos);

      for (int i = prop_start; i <= prop_end; i++) {
        if(aug_state_hist_.at(i).type == pnp){
          updatePnP(aug_state_hist_.at(i), aug_state_hist_.at(i-1));
        } else if(aug_state_hist_.at(i).type == imu){
          predictIMU(aug_state_hist_.at(i), aug_state_hist_.at(i-1));
        } else if(aug_state_hist_.at(i).type == vo){
          updateVO(aug_state_hist_.at(i),aug_state_hist_.at(i-1));
        } else{
          updateVO(aug_state_hist_.at(i), aug_state_hist_.at(i-1));
          changeAugmentedState(aug_state_hist_.at(i));
        }
        if(i == newkf_pos){
          if(aug_state_hist_.at(i).type == imu || aug_state_hist_.at(i).type == pnp){
            aug_state_hist_.at(i).type = keyframe;
            Vec6 self_tf;
            self_tf.setZero();
            aug_state_hist_.at(i).ut = self_tf;
          }
          changeAugmentedState(aug_state_hist_.at(i));
        }
      }

      latest_idx[keyframe] = newkf_pos;


    }else{
      int prop_start = min(max(latest_idx[pnp], latest_idx[vo])+1, (unsigned int)insert_pos),
              prop_end = max(max(latest_idx[pnp], latest_idx[vo]), (unsigned int)insert_pos);
      for (int i = prop_start; i <= prop_end; i++) {
        if(aug_state_hist_.at(i).type == pnp){
          updatePnP(aug_state_hist_.at(i), aug_state_hist_.at(i-1));
        } else if(aug_state_hist_.at(i).type == imu){
          predictIMU(aug_state_hist_.at(i), aug_state_hist_.at(i-1));
        } else if(aug_state_hist_.at(i).type == vo){
          updateVO(aug_state_hist_.at(i),aug_state_hist_.at(i-1));
        } else{
          updateVO(aug_state_hist_.at(i), aug_state_hist_.at(i-1));
          changeAugmentedState(aug_state_hist_.at(i));
        }
      }

    }

    latest_idx[vo] = insert_pos;
  }


  removeOldState();


  // Then and remove unnecessary states in the queue by calling removeOldState()
//  removeOldState();   // TODO

  publishFusedOdom();
  return true;

}

void EKFImuVision::repropagate(deque<AugState>::iterator& new_input_it, bool& init) {


  // TODO
  // repropagate along the queue from the new input (or keyframe when keyframe is changed), according to the type of the inputs / measurements
//  if


  // publish the latest fused odom

  publishFusedOdom();

}

void EKFImuVision::removeOldState() {

  // TODO
  // remove the unnecessary old states to prevent the queue from becoming too long
  if(aug_state_hist_.size()>10){
    int useful = min(min(latest_idx[0],latest_idx[1]), min(latest_idx[2], latest_idx[3]))-1;
    if(useful<10)
      return;

    ROS_INFO("Q before erase: %ld, useful: %d, latest: %u, %u, %u, %u", aug_state_hist_.size(), useful,
            latest_idx[0], latest_idx[1], latest_idx[2], latest_idx[3]);

    aug_state_hist_.erase(aug_state_hist_.begin(), aug_state_hist_.begin()+useful);

    latest_idx[0]-=useful;
    latest_idx[1]-=useful;
    latest_idx[2]-=useful;
    latest_idx[3]-=useful;

    ROS_INFO("Q after erase: %ld, useful: %d, latest: %u, %u, %u, %u", aug_state_hist_.size(), useful,
             latest_idx[0], latest_idx[1], latest_idx[2], latest_idx[3]);
  }
}

void EKFImuVision::predictIMU(AugState& cur_state, AugState& prev_state) {
//  ROS_INFO_STREAM("predIMU:" << prev_state.mean.head(3).transpose()<<", "<<cur_state.time_stamp.sec%100<<"+"<<cur_state.time_stamp.nsec/100000);

  // TODO
  // one of the main component in repropagate, predict new state by IMU inputs
  double dt = (cur_state.time_stamp-prev_state.time_stamp).toSec();
//  cur_state.key_frame_time_stamp = prev_state.key_frame_time_stamp;

  // copy aug states
  cur_state.mean.segment(15,6) = prev_state.mean.segment(15,6);
  Vec12 v_zeros;
  Vec15 xdot;
  v_zeros.setZero();
  xdot = modelF(prev_state.mean.segment(0,15), cur_state.ut, v_zeros);

  cur_state.mean.segment(0,15) = prev_state.mean.segment(0,15) + dt * xdot;

  Mat15x15 A_t = jacobiFx(prev_state.mean.segment(0,15), cur_state.ut, v_zeros), F_t;
  F_t = Mat15x15::Identity(15,15)+dt*A_t;

  Mat15x12 U_t = jacobiFn(prev_state.mean.segment(0,15), cur_state.ut, v_zeros), V_t;
  V_t = dt * U_t;

  cur_state.covariance.block<15,15>(0,0) = F_t * prev_state.covariance.block<15,15>(0,0) * F_t.transpose() + V_t * Qt_ * V_t.transpose();
  cur_state.covariance.block<6,15>(15,0) = prev_state.covariance.block<6,15>(15,0) * F_t.transpose();
  cur_state.covariance.block<15,6>(0,15) = F_t * prev_state.covariance.block<15,6>(0,15);
  cur_state.covariance.block<6,6>(15,15) = prev_state.covariance.block<6,6>(15,15);

}

void EKFImuVision::updatePnP(AugState& cur_state, AugState& prev_state) {
  ROS_INFO_STREAM("updaPNP:" << prev_state.mean.head(3).transpose()<<", "<<cur_state.time_stamp.sec%100+(cur_state.time_stamp.nsec*1e-9));
//  ROS_INFO_STREAM("updaPNP:" << prev_state.mean.segment(6, 3).transpose()<<", "<<cur_state.time_stamp.sec%100+(cur_state.time_stamp.nsec*1e-9));

  // TODO
  // one of the main component in repropagate, update new state by marker PnP measurements
//  cur_state.key_frame_time_stamp = prev_state.key_frame_time_stamp;

  Vec6 v_zeros;
  v_zeros.setZero();
  Vec6 z_t0 = modelG1(prev_state.mean.segment(0,15),v_zeros);
  Vec6 inno = cur_state.ut - z_t0;

  ekf_imu_vision::clampEuler(inno(3));
  ekf_imu_vision::clampEuler(inno(4));
  ekf_imu_vision::clampEuler(inno(5));

  Mat21x6 K_t;
  Mat6x21 C_t;
  Mat6x6 W_t;
  C_t.setZero();
  C_t.block<6,15>(0,0) = jacobiG1x(prev_state.mean.segment(0,15), v_zeros);
  W_t = jacobiG1v(prev_state.mean.segment(0,15), v_zeros);
  K_t = prev_state.covariance * C_t.transpose() * (C_t * prev_state.covariance * C_t.transpose() + W_t * Rt1_ * W_t.transpose()).inverse();

  cur_state.mean = prev_state.mean + K_t * inno;
  cur_state.covariance = prev_state.covariance - K_t * C_t * prev_state.covariance;

  ekf_imu_vision::clampEuler(cur_state.mean(3));
  ekf_imu_vision::clampEuler(cur_state.mean(4));
  ekf_imu_vision::clampEuler(cur_state.mean(5));

  ROS_INFO_STREAM("- after:" << cur_state.mean.head(3).transpose());
//  ROS_INFO_STREAM("- after:" << cur_state.mean.segment(6,3).transpose());

}



void EKFImuVision::updateVO(AugState& cur_state, AugState& prev_state) {
  ROS_INFO_STREAM("updatVO:" << prev_state.mean.head(3).transpose()<<", "<<cur_state.time_stamp.sec%100+(cur_state.time_stamp.nsec*1e-9));
//  ROS_INFO_STREAM("updatVO:" << prev_state.mean.segment(6,3).transpose()<<", "<<cur_state.time_stamp.sec%100+(cur_state.time_stamp.nsec*1e-9));
  // TODO
  // one of the main component in repropagate, update new state by relative pose measurements
//  cur_state.key_frame_time_stamp = prev_state.key_frame_time_stamp;

  Vec6 v_zeros;
  v_zeros.setZero();
  // prev state: the prediction state
//  cur_state.mean.segment(0,15) = prev_state.mean.segment(0,15);
  Vec6 z_t0 = modelG2(prev_state.mean, v_zeros);
  Vec6 inno = cur_state.ut - z_t0;

  ekf_imu_vision::clampEuler(inno(3));
  ekf_imu_vision::clampEuler(inno(4));
  ekf_imu_vision::clampEuler(inno(5));

  Mat21x6 K_t;
  Mat6x21 C_t;
  Mat6x6 W_t;

  C_t = jacobiG2x(prev_state.mean, v_zeros);
  W_t = jacobiG2v(prev_state.mean, v_zeros);
  K_t = prev_state.covariance * C_t.transpose() * (C_t * prev_state.covariance * C_t.transpose() + W_t * Rt2_ * W_t.transpose()).inverse();

  cur_state.mean = prev_state.mean + K_t * inno;
  cur_state.covariance = prev_state.covariance - K_t * C_t * prev_state.covariance;

  ekf_imu_vision::clampEuler(cur_state.mean(3));
  ekf_imu_vision::clampEuler(cur_state.mean(4));
  ekf_imu_vision::clampEuler(cur_state.mean(5));

  ROS_INFO_STREAM("- after:" << cur_state.mean.head(3).transpose());
//  ROS_INFO_STREAM("- after:" << cur_state.mean.segment(6,3).transpose());

}

void EKFImuVision::changeAugmentedState(AugState& state) {
  ROS_WARN("----------------change keyframe------------------------");

  // TODO
  // one of the main component in repropagate, remove the old augmented part of the state and recopy 
  state.covariance = M_a_ * M_r_ * state.covariance * M_r_.transpose() * M_a_.transpose();
  state.mean = M_a_ * M_r_ * state.mean;
  state.key_frame_time_stamp = state.time_stamp;
}

bool EKFImuVision::initFilter() {

  // TODO
  // init the filter when a keyframe after marker PnP measurements is available

  return false;
}


bool EKFImuVision::initUsingPnP(deque<AugState>::iterator start_it) {

  // TODO
  // init the state in the queue using marker PnP measurement
  

  AugState zero_state;
  Vec(21) mean;
  Mat(21, 21) covariance;
  mean.setZero();
  covariance.setIdentity();
  zero_state.mean = mean;
  zero_state.covariance = covariance;

  updatePnP(*start_it, zero_state);

  std::cout << "init PnP state: " << start_it->mean.transpose() << std::endl;


  return true;
}

// helper function converting rotation matrix to Euler angle (phi, theta, psi)
// R = Rz * Rx * Ry is assumed. If use other representation you should change the code.
Vec3 EKFImuVision::rotation2Euler(const Mat3x3& R) {
  double phi   = asin(R(2, 1));
  double theta = atan2(-R(2, 0), R(2, 2));
  double psi   = atan2(-R(0, 1), R(1, 1));
  return Vec3(phi, theta, psi);
}

void EKFImuVision::publishFusedOdom() {
//  AugState last_state = aug_state_hist_.back();
  AugState last_state = aug_state_hist_.at(max(latest_idx[pnp],latest_idx[vo]));  // always pnp actually

  double phi, theta, psi;
  phi   = last_state.mean(3);
  theta = last_state.mean(4);
  psi   = last_state.mean(5);

  if (last_state.mean.head(3).norm() > 20) {
    ROS_ERROR_STREAM("error state: " << last_state.mean.head(3).transpose());
    return;
  }

  // using the zxy euler angle
  Eigen::Quaterniond q = Eigen::AngleAxisd(psi, Eigen::Vector3d::UnitZ()) *
      Eigen::AngleAxisd(phi, Eigen::Vector3d::UnitX()) *
      Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitY());
  nav_msgs::Odometry odom;
  odom.header.frame_id = "world";
  odom.header.stamp    = last_state.time_stamp;

  odom.pose.pose.position.x = last_state.mean(0);
  odom.pose.pose.position.y = last_state.mean(1);
  odom.pose.pose.position.z = last_state.mean(2);

  odom.pose.pose.orientation.w = q.w();
  odom.pose.pose.orientation.x = q.x();
  odom.pose.pose.orientation.y = q.y();
  odom.pose.pose.orientation.z = q.z();

  odom.twist.twist.linear.x = last_state.mean(6);
  odom.twist.twist.linear.y = last_state.mean(7);
  odom.twist.twist.linear.z = last_state.mean(8);


  fuse_odom_pub_.publish(odom);

  geometry_msgs::PoseStamped path_pose;
  path_pose.header.frame_id = path_.header.frame_id = "world";
  path_pose.pose.position.x                         = last_state.mean(0);
  path_pose.pose.position.y                         = last_state.mean(1);
  path_pose.pose.position.z                         = last_state.mean(2);
  path_.poses.push_back(path_pose);
  path_pub_.publish(path_);
}




}  // namespace ekf_imu_vision

调了好几天。。。感觉没有调出来。。。。

 

测试

roslaunch aug_ekf augekf.launch

 

 

 

gwpscut
关注
  • 1
    点赞
  • 踩
  • 5
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
学习笔记之——VIO与VINS-Mono
gwpscut的博客
06-23 4724
本博文介绍一下本人团队所做的可见光通信在室内定位（包括机器人定位与人的定位）以及在IOT领域上的一些应用。先给出demo如下： 倾斜状态下的可见光定位演示
ros1版本apollo感知融合算法
最新发布
ControlLearner的博客
05-14 699
学习apollo框架设计学习apollo的感知融合算法目前只针对apollo中部分模块进行了移植，移植模块如下：使用原生ros（基于melodic）替代apollo中更改的ros使用ros pkg封装apollo中module使用catkin_make/cmake编译将ptotobuf版本提升到3.19.4将cmake版本提升到3.25.2增加动/静态障碍物rviz显示此移植版本，能很好的将自己的算法增加到框架中，应用于机器人或者无人驾驶中；
综述 | 基于特征的视觉同步定位和建图
qq_29462849的博客
04-09 4135
点击上方“计算机视觉工坊”，选择“星标”干货第一时间送达Feature‑based visual simultaneous localization and mapping: a surveyRana Azzam1 · Tarek Taha2 · Shoudong Huang3 · Yahya Zweiri4接收日期：2019 年 10 月 30 日/接受时间：2020...
超全！SLAM论文与开源代码汇总（激光+视觉+融合）
3D视觉工坊
10-21 7639
1.代表性视觉SLAM算法论文与开源代码总结2.代表性激光SLAM算法论文与开源代码总结3.代表性激光-视觉融合SLAM算法论文总结激光-视觉-IMU-GPS融合SLAM算法理论与代码讲解...
深入理解如何不费吹灰之力搭建一个无人驾驶车（三）2D-小车拓展部分（AMCL+EKF）（EKF协方差矩阵如何写？）
qq_38588806的博客
03-05 5174
三、2D小车定位拓展 本文讲一下不是很难的高中部分，主要是AMCL及EKF，两种定位方法 3.1 AMCL定位 首先讲一下AMCL算法，自适应蒙特卡罗粒子滤波定位，其实在gmapping中，我们就要用轮式里程计的odom产生粒子，有些东西，讲大家可能不懂，可以见概率机器人或机器人学状态估计，在贝叶斯滤波中，置信度的更新是靠观测，即一个归一化系数*P(Zt|xt)*预测置信度bel(xt)，这个预测...
同时校准移动机器人的里程表和传感器参数
qq_17032807的博客
06-27 5153
Simultaneous calibration of odometry and sensor parameters for mobile robots，Andrea Censi, Student Member, Antonio Franchi, Member, Luca Marchionni, and Giuseppe Oriolo, Senior Member，2013，IEEE Trans...
ros navigation翻译
HamRadioDXer的QQ空间
11-07 116
限于本人非常菜，所谓的实验室也没个人指导，只好闭门造车瞎琢磨，毕业也不会再从事相关相关工作（主要是也轮不到自己O(∩_∩)O哈哈~） 本篇博客也只是一个从入门到入门从未精通最后到放弃的菜鸡自言自语，看了浪费时间哈哈 首先是markdown编辑器的选择，自己习惯了有道云，可是不能直接上传图片不提供图床.github的图床又非常慢。琢磨了一上午【小书匠】还是没搞懂github图床怎么能快点，并且...
SLAMCN资料收藏转载
zhangrelay的专栏
11-22 2995
网页链接地址：http://www.slamcn.org/index.php/%E9%A6%96%E9%A1%B5资料非常丰富，内容如下：首页目录 [隐藏] 1 SLAM 介绍1.1 什么是SLAM1.2 SLAM与视觉里程计（Visual Odometry）1.3 SLAM和SfM2 主流开源SLAM方案2.1 视觉传感器2.2 激光传感器2.3 视觉(Visual)与IMU融合(VI)2.4 
机器人工程本科专业课教学资源汇总（2018年暑假补充学习用）
zhangrelay的专栏
07-24 7899
手机应用软件：Robotics Engineering - Apps on Google Play This Robotics Engineering App provides the basic know-how on the foundations of robotics: modelling, planning and control. The App takes the user thr...
s-msckf代码笔记（二）
OORRANNGGE的博客
05-10 2387
二、VIO VIO中IMU的状态向量为： xI=(GIqTbgTGvITbaTGpITCIqTIpCT)T \textbf x_I=\begin{pmatrix} {}_G^I\textbf q^T &amp; \textbf b_g^T &amp; {}^G\textbf v_I^T &amp; \textbf b_a^T &amp; {}^G\textbf p_...
基于ROS的多传感器融合感知系统实现（雷达+相机）
03-06
总之，基于ROS的多传感器融合感知系统，通过结合雷达和相机的优势，能够实现更高效、更准确的环境感知，为机器人提供更强大的导航和决策能力。实际应用中，需不断调整和优化融合算法，以适应不同的应用场景和需求。
ROS学习笔记之——多机器人探索环境
01-06
本博文是基于这个ROS软件包（https://github.com/hrnr/m-explore）的学习笔记 目录 multi robot exploration nav_msgs/OccupancyGrid  map_msgs/OccupancyGridUpdate move_base multirobot_map_merge 参考资料 ...
ROS复习笔记之——参数的使用
01-06
《ROS复习笔记之——系统框架及其编程规范》 《 ROS复习笔记之——创建和运行服务服务器与客户端节点》 《ROS复习笔记之——创建和运行动作服务器与客户端节点》 修改博客《 ROS复习笔记之——创建和运行服务...
ROS复习笔记之——创建和运行动作服务器与客户端节点
01-07
博客《 ROS复习笔记之——创建和运行服务服务器与客户端节点》复习了创建和运行服务服务器与客户端节点。本博文继这个博客，来复习一下创建和运行动作服务器与客户端节点。 动作与话题和服务不同，在异步、双向，...
ROS复习笔记之——roslaunch的应用
01-20
`roslaunch` 是 ROS 中的一个核心工具，它允许用户通过一个简单的命令行接口启动多个节点、执行配置设置以及管理复杂的启动过程。这篇复习笔记将深入探讨 `roslaunch` 的应用，特别是如何创建和使用 `launch` 文件。...
学习笔记之——激光雷达SLAM（LOAM系列的复现与学习）
热门推荐
gwpscut的博客
08-17 2万+
For cooperaive inquiries, please visit the website：guanweipeng.com
学习笔记之——Event Camera（事件相机）调研
gwpscut的博客
04-20 2万+
概述 Event Camera是一种bio-inspired sensor，对应的领域为bio-inspired vision。对于传统的相机，从某种程度上，是捕获一个静态/静止的空间，而Event Camera的目的是：敏感地捕捉运动的物体。 对于传统RGB-camera，相机传回的信息是同步的，所谓同步，就是在某一时刻t，相机会进行曝光，把这一时刻所有的像素填在一个矩阵里回传，一张照片就诞生了。一张照片上所有的像素都对应着同一时刻。至于视频，不过是很多帧的图片，相邻图片间的时间间隔...
ROS学习笔记之——路径规划及avoid obstacles
gwpscut的博客
11-29 1万+
之前博客《ROS学习笔记之——Navigation Stack及路径规划》介绍了navigation stack，其中涉及到的amcl以及避障还没有详细的展开 AMCL 避障（avoid obstales） Robot Navigation Challenge 参考资料 http://wiki.ros.org/amcl 不错的视频学习链接（https://www.youtube.com/r...
基于ros的多传感器数据融合
11-22
基于ROS的多传感器数据融合是指通过机器人操作系统（ROS）来集成和处理从多个传感器获取的数据，以产生更准确、全面和可靠的环境感知结果。 首先，ROS提供了一种灵活的通信机制，使得不同传感器设备之间可以互相通信和交换数据。这使得我们可以方便地将各种传感器集成到一个ROS系统中，包括摄像头、激光雷达、惯性测量单元（IMU）等。 其次，ROS具有强大的数据处理和融合功能。多传感器数据融合的目标是将来自不同传感器的信息进行有效地组合，提高环境感知的准确性和可靠性。通过ROS的数据处理和融合功能，我们可以利用各种算法和技术，如滤波、配准、特征提取等，对传感器数据进行融合和处理，从而得到更全面和一致的环境感知结果。 最后，基于ROS的多传感器数据融合还可以为机器人系统提供更高级的功能。通过将多个传感器的输出结果整合起来，我们可以实现更复杂的任务，如自主导航、目标跟踪、环境建图等。这些高级功能可以通过ROS的导航、感知和控制模块来实现，使得机器人系统在各种环境和任务中表现更加出色。 综上所述，基于ROS的多传感器数据融合可以帮助我们更好地利用传感器设备，提高环境感知的准确性和可靠性，并为机器人系统提供更高级的功能。这对于在各种应用领域中，如自动驾驶、工业自动化、农业机器人等都具有重要意义。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值