ROS中EKF（扩展卡尔曼跟踪）的使用

1.代码

• 位置，速度类：state_pos_vel.h

#ifndef STATE_POS_VEL_H
#define STATE_POS_VEL_H

#include <tf/tf.h>

namespace BFL
{
/// Class representing state with pos and vel
class StatePosVel
{
public:
  tf::Vector3 pos_, vel_;
  /// Constructor
  StatePosVel(const tf::Vector3& pos = tf::Vector3(0, 0, 0),
              const tf::Vector3& vel = tf::Vector3(0, 0, 0)):  pos_(pos), vel_(vel) {};
  /// Destructor
  ~StatePosVel() {};
  /// operator +=
  StatePosVel& operator += (const StatePosVel& s)
  {
    this->pos_ += s.pos_;
    this->vel_ += s.vel_;
    return *this;
  }
  /// operator +
  StatePosVel operator + (const StatePosVel& s)
  {
    StatePosVel res;
    res.pos_ = this->pos_ + s.pos_;
    res.vel_ = this->vel_ + s.vel_;
    return res;
  }
  /// output stream for StatePosVel
  friend std::ostream& operator<< (std::ostream& os, const StatePosVel& s)
  {
    os << "(" << s.pos_[0] << ", " << s.pos_[1] << ", "  << s.pos_[2] << ")--("
       << "(" << s.vel_[0] << ", " << s.vel_[1] << ", "  << s.vel_[2] << ") ";
    return os;
  };
};
} // end namespace
#endif

• 跟踪基类：tracker.h

#ifndef __TRACKER__
#define __TRACKER__

#include "state_pos_vel.h"
#include <bfl/wrappers/matrix/matrix_wrapper.h>
#include <string>


namespace estimation
{
class Tracker
{
public:
  /// constructor
  Tracker(const std::string& name): name_(name) {};
  /// destructor
  virtual ~Tracker() {};
  /// return the name of the tracker
  const std::string& getName() const
  {
    return name_;
  };
  /// initialize tracker
  virtual void initialize(const BFL::StatePosVel& mu, const BFL::StatePosVel& sigma, const double time) = 0;
  /// return if tracker was initialized
  virtual bool isInitialized() const = 0;
  /// return measure for tracker quality: 0=bad 1=good
  virtual double getQuality() const = 0;
  /// return the lifetime of the tracker
  virtual double getLifetime() const = 0;
  /// return the time of the tracker
  virtual double getTime() const = 0;
  /// update tracker
  virtual bool updatePrediction(const double time) = 0;
  virtual bool updateCorrection(const tf::Vector3& meas,
                                const MatrixWrapper::SymmetricMatrix& cov) = 0;
  /// get filter posterior
  virtual void getEstimate(BFL::StatePosVel& est) const = 0;
private:
  std::string name_;
}; // class
}; // namespace

#endif

• 卡尔曼跟踪类：tracker_kalman.h

#ifndef __TRACKER_KALMAN__
#define __TRACKER_KALMAN__

#include "tracker.h"
// bayesian filtering
#include <bfl/filter/extendedkalmanfilter.h>
#include <bfl/model/linearanalyticsystemmodel_gaussianuncertainty.h>
#include <bfl/model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
#include <bfl/pdf/linearanalyticconditionalgaussian.h>
#include "state_pos_vel.h"
// TF
#include <tf/tf.h>
// log files
#include <fstream>

namespace estimation
{
class TrackerKalman: public Tracker
{
public:
  /// constructor
  TrackerKalman(const std::string& name, const BFL::StatePosVel& sysnoise);
  /// destructor
  virtual ~TrackerKalman();
  /// initialize tracker
  virtual void initialize(const BFL::StatePosVel& mu, const BFL::StatePosVel& sigma, const double time);
  /// return if tracker was initialized
  virtual bool isInitialized() const
  {
    return tracker_initialized_;
  };
  /// return measure for tracker quality: 0=bad 1=good
  virtual double getQuality() const
  {
    return quality_;
  };
  /// return the lifetime of the tracker
  virtual double getLifetime() const;
  /// return the time of the tracker
  virtual double getTime() const;
  /// update tracker
  virtual bool updatePrediction(const double time);
  virtual bool updateCorrection(const tf::Vector3& meas,
                                const MatrixWrapper::SymmetricMatrix& cov);
  /// get filter posterior
  virtual void getEstimate(BFL::StatePosVel& est) const;
private:
  // pdf / model / filter
  BFL::Gaussian                                           prior_;
  BFL::ExtendedKalmanFilter*                              filter_;
  BFL::LinearAnalyticConditionalGaussian*                 sys_pdf_;
  BFL::LinearAnalyticSystemModelGaussianUncertainty*      sys_model_;
  BFL::LinearAnalyticConditionalGaussian*                 meas_pdf_;
  BFL::LinearAnalyticMeasurementModelGaussianUncertainty* meas_model_;
  MatrixWrapper::Matrix                                   sys_matrix_;
  MatrixWrapper::SymmetricMatrix                          sys_sigma_;
  double calculateQuality();
  // vars
  bool tracker_initialized_;
  double init_time_, filter_time_, quality_;
}; // class
}; // namespace
#endif

tracker_kalman.cpp

#include "tracker_kalman.h"
#include <iostream>

using namespace MatrixWrapper;
using namespace BFL;
using namespace tf;
using namespace std;
using namespace ros;


const static double damping_velocity = 0.9;


namespace estimation
{
// constructor
TrackerKalman::TrackerKalman(const string& name, const StatePosVel& sysnoise):
  Tracker(name),
  filter_(NULL),
  sys_pdf_(NULL),
  sys_model_(NULL),
  meas_pdf_(NULL),
  meas_model_(NULL),
  sys_matrix_(6, 6),
  tracker_initialized_(false)
{
  // create sys model
  sys_matrix_ = 0; // F
  for (unsigned int i = 1; i <= 3; i++)
  {
    sys_matrix_(i, i) = 1;
    sys_matrix_(i + 3, i + 3) = damping_velocity;
  }
  // Q
  ColumnVector sys_mu(6);
  sys_mu = 0;
  sys_sigma_ = SymmetricMatrix(6);
  sys_sigma_ = 0;
  for (unsigned int i = 0; i < 3; i++)
  {
    sys_sigma_(i + 1, i + 1) = pow(sysnoise.pos_[i], 2);
    sys_sigma_(i + 4, i + 4) = pow(sysnoise.vel_[i], 2);
  }
  Gaussian sys_noise(sys_mu, sys_sigma_);
  sys_pdf_   = new LinearAnalyticConditionalGaussian(sys_matrix_, sys_noise);
  sys_model_ = new LinearAnalyticSystemModelGaussianUncertainty(sys_pdf_);

  // create meas model
  // H
  Matrix meas_matrix(3, 6);
  meas_matrix = 0;
  for (unsigned int i = 1; i <= 3; i++)
    meas_matrix(i, i) = 1;
  // R
  ColumnVector meas_mu(3);
  meas_mu = 0;
  SymmetricMatrix meas_sigma(3);
  meas_sigma = 0;
  for (unsigned int i = 0; i < 3; i++)
    meas_sigma(i + 1, i + 1) = 0;
  Gaussian meas_noise(meas_mu, meas_sigma);
  meas_pdf_   = new LinearAnalyticConditionalGaussian(meas_matrix, meas_noise);
  meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(meas_pdf_);
};

// destructor
TrackerKalman::~TrackerKalman()
{
  if (filter_)      delete filter_;
  if (sys_pdf_)     delete sys_pdf_;
  if (sys_model_)   delete sys_model_;
  if (meas_pdf_)    delete meas_pdf_;
  if (meas_model_)  delete meas_model_;
};

// initialize prior density of filter
void TrackerKalman::initialize(const StatePosVel& mu, const StatePosVel& sigma, const double time)
{
  ColumnVector mu_vec(6);
  SymmetricMatrix sigma_vec(6);
  sigma_vec = 0;
  for (unsigned int i = 0; i < 3; i++)
  {
    mu_vec(i + 1) = mu.pos_[i];
    mu_vec(i + 4) = mu.vel_[i];
    sigma_vec(i + 1, i + 1) = pow(sigma.pos_[i], 2);
    sigma_vec(i + 4, i + 4) = pow(sigma.vel_[i], 2);
  }
  prior_ = Gaussian(mu_vec, sigma_vec);
  filter_ = new ExtendedKalmanFilter(&prior_);

  // tracker initialized
  tracker_initialized_ = true;
  quality_ = 1;
  filter_time_ = time;
  init_time_ = time;
}

// update filter prediction
bool TrackerKalman::updatePrediction(const double time)
{
  bool res = true;
  if (time > filter_time_)
  {
    // set dt in sys model
    for (unsigned int i = 1; i <= 3; i++)
      sys_matrix_(i, i + 3) = time - filter_time_;
    sys_pdf_->MatrixSet(0, sys_matrix_);

    // scale system noise for dt
    sys_pdf_->AdditiveNoiseSigmaSet(sys_sigma_ * pow(time - filter_time_, 2));
    filter_time_ = time;

    // update filter
    res = filter_->Update(sys_model_);
    if (!res) quality_ = 0;
    else quality_ = calculateQuality();
  }
  return res;
};

// update filter correction
bool TrackerKalman::updateCorrection(const tf::Vector3&  meas, const MatrixWrapper::SymmetricMatrix& cov)
{
  assert(cov.columns() == 3);

  // copy measurement
  ColumnVector meas_vec(3);
  for (unsigned int i = 0; i < 3; i++)
    meas_vec(i + 1) = meas[i];

  // set covariance
  ((LinearAnalyticConditionalGaussian*)(meas_model_->MeasurementPdfGet()))->AdditiveNoiseSigmaSet(cov);

  // update filter
  bool res = filter_->Update(meas_model_, meas_vec);
  if (!res) quality_ = 0;
  else quality_ = calculateQuality();

  return res;
};

void TrackerKalman::getEstimate(StatePosVel& est) const
{
  ColumnVector tmp = filter_->PostGet()->ExpectedValueGet();
  for (unsigned int i = 0; i < 3; i++)
  {
    est.pos_[i] = tmp(i + 1);
    est.vel_[i] = tmp(i + 4);
  }
};

double TrackerKalman::calculateQuality()
{
  double sigma_max = 0;
  SymmetricMatrix cov = filter_->PostGet()->CovarianceGet();
  for (unsigned int i = 1; i <= 2; i++)
    sigma_max = max(sigma_max, sqrt(cov(i, i)));

  return 1.0 - min(1.0, sigma_max / 1.5);
}

double TrackerKalman::getLifetime() const
{
  if (tracker_initialized_)
    return filter_time_ - init_time_;
  else
    return 0;
}

double TrackerKalman::getTime() const
{
  if (tracker_initialized_)
    return filter_time_;
  else
    return 0;
}

}; // namespace

• 主文件：main.cpp

#include <iostream>
#include "tracker_kalman.h"

using namespace MatrixWrapper;
using namespace estimation;
using namespace BFL;
using namespace tf;
using namespace std;

int main() {
  // system noise
  BFL::StatePosVel sys_sigma_(Vector3(0.05, 0.05, 0.05),
                              Vector3(1.0, 1.0, 1.0));
  TrackerKalman filter("tracker_name", sys_sigma_);
  // EKF prior density
  StatePosVel prior_sigma(Vector3(0.1, 0.1, 0.1),
                          Vector3(0.0000001, 0.0000001, 0.0000001));
  StatePosVel mu(Vector3(0, 0, 0), Vector3(0.5, 0.5, 0));
  filter.initialize(mu, prior_sigma, 0);
  SymmetricMatrix cov(3);
  cov = 0.0;
  cov(1, 1) = 0.0025;
  cov(2, 2) = 0.0025;
  cov(3, 3) = 0.0025;
  for (int i = 1; i < 10; ++i) {
    filter.updatePrediction(1 * i);
    filter.updateCorrection(Vector3(0.22*i, 0.09*i, 0), cov);
    StatePosVel est;
    filter.getEstimate(est);
    cout << est << endl;
  }

  return 0;
}

2.当TrackerKalman作为类的成员变量时

#include <iostream>
#include <list>
#include "tracker_kalman.h"

using namespace MatrixWrapper;
using namespace estimation;
using namespace BFL;
using namespace tf;
using namespace std;

class Test {
 public:
  TrackerKalman filter;
  StatePosVel sys_sigma_;
  int test;
  Test(int test)
      : sys_sigma_(Vector3(0.05, 0.05, 0.05), Vector3(1.0, 1.0, 1.0)),
        filter("tracker_name", sys_sigma_) {
    StatePosVel prior_sigma(Vector3(0.1, 0.1, 0.1),
                            Vector3(0.0000001, 0.0000001, 0.0000001));
    StatePosVel mu(Vector3(0, 0, 0), Vector3(0.5, 0.5, 0));
    filter.initialize(mu, prior_sigma, 0);
    this->test = test;
  }
  ~Test() {}
  void Propagate(float time) {
    filter.updatePrediction(time);
  }
  void update(Vector3 meas, SymmetricMatrix cov) {
    filter.updateCorrection(meas, cov);
  }
  void getState(StatePosVel &est) {
    filter.getEstimate(est);
  }
};

int main() {               
  list<Test> l;
  l.push_back(Test(1));
  list<Test>::iterator i = l.begin();
  cout << i->test << endl;
  // i->Propagate(1);
  Test tt(6);
  tt.Propagate(0.6);
  Test *t = new Test(6);
  SymmetricMatrix cov(3);
  cov = 0.0;
  cov(1, 1) = 0.0025;
  cov(2, 2) = 0.0025;
  cov(3, 3) = 0.0025;
  for (int i = 1; i < 10; ++i) {
    t->Propagate(1 * i);
    t->update(Vector3(0.22 * i, 0.09 * i, 0), cov);
    StatePosVel est;
    t->getState(est);
    cout << est << endl;
  }

  return 0;
}

• main函数中i->Propagate(1);不注释，会有段错误，注释则会在循环之后出现段错误。所以要使用指针，即list<Test*> l;，原因不明。