MRPT（移动机器人编程工具包）卡尔曼滤波器

Question

 我是 SLAM 领域的新手。最近我发现了移动机器人编程工具包，我想学习其中的扩展卡尔曼滤波器。但令人沮丧的是，我认为示例 bayesianTracking/test.cpp 对我来说有点复杂。所以，我调整它来解决以下一个简单的问题：

 此示例在假设速度和高度恒定的情况下跟踪飞机的位置，并测量到飞机的倾斜距离。这意味着我们需要 3 个状态变量 - 水平距离、水平速度和高度：

 X=[x, vx, y]

 测量功能为：

 h(x)=sqrt(x^2+y^2)

 对于这个问题，我们有观察雅可比行列式：

 J_H=[x/sqrt(x^2+y^2), 0, y/sqrt(x^2+y^2)]

 我的代码中没有语法错误，但是结果不收敛。也许我犯了一些错误？有人可以帮我找出问题出在哪里吗？

#include <mrpt/bayes/CKalmanFilterCapable.h>
#include <mrpt/random.h>
#include <mrpt/system/os.h>
#include <mrpt/system/threads.h>
#include <iostream>

using namespace mrpt;
using namespace mrpt::bayes;
using namespace mrpt::math;
using namespace mrpt::utils;
using namespace mrpt::random;
using namespace std;

#define DELTA_TIME                  0.05f   // Time Step between Filter Steps
#define VEHICLE_INITIAL_X           10.0f
#define VEHICLE_INITIAL_Y           2000.0f
#define VEHICLE_INITIAL_V           200.0f
#define TRANSITION_MODEL_STD        1.0f    
#define RANGE_SENSOR_NOISE_STD      5.0f    


// Implementation of the system models as a EKF
class CRange: public CKalmanFilterCapable<3, 1, 0, 0>
{
public:
  CRange( );
  virtual ~CRange();

  void  Process( double DeltaTime, double observationRange);

  void getState( KFVector &xkk, KFMatrix &pkk)
  { 
      xkk = m_xkk;  //The system state vector.
      pkk = m_pkk;  //The system full covariance matrix
  }


protected:
  float m_obsRange; 
  float m_deltaTime;    // Time Step between Filter Steps

  // return the action vector u
  void OnGetAction( KFArray_ACT &out_u ) const;  

  // Implements the transition model 
  void OnTransitionModel(const KFArray_ACT &in_u,KFArray_VEH &inout_x,bool &out_skipPrediction) const;

  // Implements the transition Jacobian
  void OnTransitionJacobian(KFMatrix_VxV  &out_F ) const;

  // Implements the transition noise covariance
  void OnTransitionNoise(KFMatrix_VxV &out_Q ) const;

  // Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
  void OnGetObservationNoise(KFMatrix_OxO &out_R) const;

  /** This is called between the KF prediction step and the update step
  *  This method will be called just once for each complete KF iteration.
  * \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.
  */
  void OnGetObservationsAndDataAssociation(
      vector_KFArray_OBS            &out_z,
      mrpt::vector_int            &out_data_association,
      const vector_KFArray_OBS  &in_all_predictions,
      const KFMatrix              &in_S,
      const vector_size_t         &in_lm_indices_in_S,
      const KFMatrix_OxO          &in_R
      );

  // Implements the observation prediction 
  void OnObservationModel(const vector_size_t &idx_landmarks_to_predict,vector_KFArray_OBS  &out_predictions) const;

  // Implements the observation Jacobians
  void OnObservationJacobians(const size_t &idx_landmark_to_predict,KFMatrix_OxV &Hx,KFMatrix_OxF &Hy) const;
};


CRange::CRange()
{
  KF_options.method = kfEKFNaive;

  //  State: (x,vx,y)
  m_xkk.resize(3);  
  m_xkk[0]= VEHICLE_INITIAL_X;
  m_xkk[1]= VEHICLE_INITIAL_V;
  m_xkk[2]= VEHICLE_INITIAL_Y;

  // Initial cov:  Large uncertainty
  m_pkk.setSize(3,3); 
  m_pkk.unit();
  m_pkk = 50 * m_pkk;
}


CRange::~CRange()
{

}


void  CRange::Process( double DeltaTime, double observationRange)
{
  m_deltaTime = (float)DeltaTime;
  m_obsRange  = (float)observationRange;

  runOneKalmanIteration(); // executes one complete step: prediction + update
}


// Must return the action vector u.
// param out_u: The action vector which will be passed to OnTransitionModel
void CRange::OnGetAction( KFArray_ACT &out_u ) const
{

}


/** Implements the transition model(Project the state ahead)
 param in_u :    The vector returned by OnGetAction.
 param inout_x:  prediction value
 param out_skip: Set this to true if for some reason you want to skip the prediction step. Default:false
  */
void CRange::OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
{
  // The constant-velocities model is implemented simply as:
  inout_x[0] += (inout_x[0] + m_deltaTime * inout_x[1]);
  inout_x[1] = inout_x[1];
  inout_x[2] = inout_x[2];
}


/** Implements the transition Jacobian 
param out_F Must return the Jacobian.
The returned matrix must be N*N with N being the size of the whole state vector.
*/
void CRange::OnTransitionJacobian(KFMatrix_VxV  &F) const
{
  F.unit();
  F(0,1) = m_deltaTime;
}


/** Implements the transition noise covariance 
 param out_Q Must return the covariance matrix.
 The returned matrix must be of the same size than the jacobian from OnTransitionJacobian
  */
void CRange::OnTransitionNoise(KFMatrix_VxV &Q) const
{
  Q.unit();
  Q *= square(TRANSITION_MODEL_STD);
}


/** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
param out_R : The noise covariance matrix. It might be non diagonal, but it'll usually be.
*/
void CRange::OnGetObservationNoise(KFMatrix_OxO &R) const
{
  R.unit();
  R *= square(RANGE_SENSOR_NOISE_STD);
}


// This is called between the KF prediction step and the update step
void CRange::OnGetObservationsAndDataAssociation(
    vector_KFArray_OBS          &out_z,
    mrpt::vector_int            &out_data_association,
    const vector_KFArray_OBS    &in_all_predictions,
    const KFMatrix              &in_S,
    const vector_size_t         &in_lm_indices_in_S,
    const KFMatrix_OxO          &in_R)
{
  //out_z: N vectors, N being the number of "observations"
  out_z.resize(1);
  out_z[0][0] = m_obsRange;
}


/** Implements the observation prediction 
param idx_landmark_to_predict: The indices of the landmarks in the map whose predictions are expected as output. For non SLAM-like problems, this input value is undefined and the application should just generate one observation for the given problem.
param out_predictions: The predicted observations.
*/
void CRange::OnObservationModel(const vector_size_t &idx_landmarks_to_predict,vector_KFArray_OBS &out_predictions) const
{
  // idx_landmarks_to_predict is ignored in NON-SLAM problems
  out_predictions.resize(1);
  out_predictions[0][0] = sqrt( square(m_xkk[0]) + square(m_xkk[2]) );
}


// Implements the observation Jacobians 
void CRange::OnObservationJacobians(const size_t &idx_landmark_to_predict,KFMatrix_OxV &Hx,KFMatrix_OxF &Hy) const
{
  Hx.zeros();
  Hx(0,0) = m_xkk[0] / sqrt(square(m_xkk[0])+square(m_xkk[2]));
  Hx(0,2) = m_xkk[2] / sqrt(square(m_xkk[0])+square(m_xkk[2]));
}


int main ()
{
  // Create class instance
  CRange    EKF;
  EKF.KF_options.method = kfEKFNaive; //select the KF algorithm

  // Initiate simulation
  float x=0, y=1000, v=100; // true values
  float t=0;

  while (!mrpt::system::os::kbhit())
  { 
      // Simulate noisy observation:
      x += v * DELTA_TIME;
      float realRange = sqrt(square(x)+square(y));

      // double mrpt::random::CRandomGenerator::drawGaussian1D_normalized(double * likelihood = NULL)   
      // Generate a normalized (mean=0, std=1) normally distributed sample
      float obsRange = max(0.0, realRange + RANGE_SENSOR_NOISE_STD * randomGenerator.drawGaussian1D_normalized() );

      printf("Real/Simulated range: %.03f / %.03f \n", realRange, obsRange );


      // Process with EKF
      EKF.Process(DELTA_TIME, obsRange);


      // Show EKF state:
      CRange::KFVector EKF_xkk;
      CRange::KFMatrix EKF_pkk;
      EKF.getState( EKF_xkk, EKF_pkk );

      printf("Real state: x:%.03f  v=%.03f  y=%.03f \n",x,v,y);
      cout << "EKF estimation:" <<endl<< EKF_xkk << endl;
      cout <<"-------------------------------------------"<<endl;

      // Delay(An OS-independent method for sending the current thread to "sleep" for a given period of time)
      mrpt::system::sleep((int)(DELTA_TIME*1000));
      t += DELTA_TIME;
  }


  return 0;
}

Answer 1

转换模型中x[0]的更新错误，inout_x[0]被添加了两次。这应该有效：

void CRange::OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
{
  // The constant-velocities model is implemented simply as:
  inout_x[0] += m_deltaTime * inout_x[1];
  inout_x[1] = inout_x[1];
  inout_x[2] = inout_x[2];
}