extendedKalmanFilter.h

Go to the documentation of this file.
00001 
00015 #ifndef DLR_COMPUTERVISION_EXTENDEDKALMANFILTER_H
00016 #define DLR_COMPUTERVISION_EXTENDEDKALMANFILTER_H
00017 
00018 #include <dlrNumeric/array1D.h>
00019 #include <dlrNumeric/array2D.h>
00020 
00021 namespace dlr {
00022 
00023   namespace computerVision {
00024 
00025     
00033     class ExtendedKalmanFilter {
00034     public:
00035 
00039       explicit
00040       ExtendedKalmanFilter(double startTime = 0.0);
00041 
00042 
00046       virtual
00047       ~ExtendedKalmanFilter() {};
00048 
00049 
00068       virtual void
00069       addMeasurement(unsigned int measurementID,
00070                      double timestamp,
00071                      numeric::Array1D<double> const& measurement,
00072                      numeric::Array1D<double> const& controlInput);
00073 
00074 
00103       virtual void
00104       checkMeasurementJacobians(unsigned int measurementID,
00105                                 numeric::Array1D<double> const& epsilonArray,
00106                                 numeric::Array2D<double>& residualArray0);
00107       
00108       
00138       virtual void
00139       checkProcessJacobians(numeric::Array1D<double> const& epsilonArray,
00140                             numeric::Array1D<double> const& controlInput,
00141                             numeric::Array2D<double>& residualArray);
00142       
00143       
00162       void
00163       doPredictionStep(double currentTime,
00164                        Array1D<double> const& controlInput);
00165 
00166 
00182       void
00183       doMeasurementUpdate(unsigned int measurementID,
00184                           numeric::Array1D<double> const& measurement);
00185 
00186 
00195       virtual void
00196       freezeKalmanGain();
00197 
00198       
00212       virtual void
00213       getStateEstimate(double& timestamp,
00214                        numeric::Array1D<double>& state,
00215                        numeric::Array2D<double>& covariance);
00216 
00217       
00231       virtual void
00232       setStateEstimate(double timestamp,
00233                        numeric::Array1D<double> const& state,
00234                        numeric::Array2D<double> const& covariance);
00235 
00236       
00244       virtual void
00245       unfreezeKalmanGain();
00246       
00247     protected:
00248 
00276       virtual
00277       numeric::Array1D<double>
00278       applyMeasurementModel(unsigned int measurementID,
00279                             double currentTime,
00280                             double previousTime,
00281                             numeric::Array1D<double> const& currentState) = 0;
00282 
00283       
00311       virtual
00312       numeric::Array1D<double>
00313       applyProcessModel(double currentTime,
00314                         double previousTime,
00315                         numeric::Array1D<double> const& previousState,
00316                         numeric::Array1D<double> const& controlInput) = 0;
00317 
00318       
00349       virtual
00350       void
00351       getMeasurementJacobians(unsigned int measurementID,
00352                               double currentTime,
00353                               double previousTime,
00354                               numeric::Array1D<double> const& state,
00355                               numeric::Array2D<double>& stateJacobian,
00356                               numeric::Array2D<double>& noiseJacobian) = 0;
00357 
00358 
00385       virtual
00386       void
00387       getProcessJacobians(double currentTime,
00388                           double previousTime,
00389                           numeric::Array1D<double> const& state,
00390                           numeric::Array2D<double>& stateJacobian,
00391                           numeric::Array2D<double>& noiseJacobian) = 0;
00392 
00393 
00410       virtual
00411       numeric::Array2D<double>
00412       getMeasurementNoiseCovariance(unsigned int measurementID,
00413                                     double currentTime,
00414                                     double previousTime) = 0;
00415 
00416       
00429       virtual
00430       numeric::Array2D<double>
00431       getProcessNoiseCovariance(double currentTime, double previousTime) = 0;
00432 
00433 
00434       /* =================== Member variables =================== */
00435 
00436       numeric::Array2D<double> m_covariance;
00437       numeric::Array1D<double> m_state;
00438       double m_previousTimestamp;
00439       double m_timestamp;
00440     };
00441 
00442     
00443   } // namespace computerVision
00444   
00445 } // namespace dlr
00446 
00447 
00448 
00449 /* ============ Definitions of inline & template functions ============ */
00450 
00451 
00452 namespace dlr {
00453 
00454   namespace computerVision {
00455 
00456     
00457   } // namespace computerVision
00458   
00459 } // namespace dlr
00460 
00461 #endif /* #ifndef DLR_COMPUTERVISION_EXTENDEDKALMANFILTER_H */