I'm new to the domain of SLAM. Recently I found the Mobile Robot Programming Toolkit and I want to learn the extended Kalman filter in it.But frustratedly I think the sample bayesianTracking/test.cpp is a bit complex to me. So,I adapt it to solve a simple problem below:
This example tracks the position of an aircraft assuming a constant velocity and altitude, and measurements of the slant distance to the aircraft.That means we need 3 state variables - horizontal distance, horizonal velocity, and altitude:
X=[x, vx, y]
The measurement function is:
h(x)=sqrt(x^2+y^2)
For this problem we have observation Jacobians:
J_H=[x/sqrt(x^2+y^2), 0, y/sqrt(x^2+y^2)]
There is no syntax error in my code, but the results do not converge. Maybe I made some mistakes? Can someone help me find out where is the problem?
#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;
}
The update of x[0] in the transition model is wrong, inout_x[0] is added twice. This should work:
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];
}