KalmanPendulum.h

#ifndef KALMAN_PENDULUM_H
#define KALMAN_PENDULUM_H
 
#include <fhsim/simobject/KalmanObject.h>
#include "Pendulum.h"
#include <fhsim/simobject/SimObject.h>
 
#include "sfh/constants.h"
#include "sfh/math.h"
#include "sfh/text.h"
#include "sfh/util.h"
 
#include <Eigen/Dense>
 
#include <memory>
 
class KalmanPendulum : public Pendulum, public KalmanObject
{
   public:
   KalmanPendulum(const std::string& simObjectName, ISimObjectCreator* const creator);
   estimator::Matrix GetInitialEstimationErrorCovariance();
   estimator::Matrix GetProcessNoiseCovariance();
   estimator::Matrix GetMeasurementNoiseCovariance();
   estimator::Vector ExpectedMeasurementOutput(const double T, const double* const X);
   estimator::Vector ActualMeasurementOutput(const double T, const double* const X);
   unsigned int GetNumMeasurements();
 
   protected:
   double m_AngleUncertainty;
   double m_AngularVelocityUncertainty;
   double m_MeasurementUncertainty;
   double m_InitialP[2];
   ISignalPort* m_AngleMeasurement;
};
 
#endif