00001 /************************************************************************************************/ 00002 /*! \file KalmanFilter.h 00003 * \brief The KalmanFilter class implements a linear Kalman Filter. 00004 * \author $Author: cakinli $ 00005 * \version $Revision: 1.18 $ 00006 * \date $Date: 2005/02/25 18:40:54 $ 00007 ************************************************************************************************/ 00008 /*! 00009 * 00010 ************************************************************************************************/ 00011 00012 #ifndef __SSSL_KALMANFILTER_H__ 00013 #define __SSSL_KALMANFILTER_H__ 00014 00015 #include <Filtering/SequentialFilter.h> 00016 00017 using namespace std; 00018 using namespace O_SESSAME; 00019 00020 class KalmanFilter : public SequentialFilter { 00021 00022 public: 00023 00024 // Contructors/Deconstructors 00025 00026 /*! Default constructor */ 00027 KalmanFilter(); 00028 00029 /*! Kalman data constructor */ 00030 KalmanFilter(Matrix& _covariance, Matrix& _kalmanGain); 00031 00032 /*! Deconstructor */ 00033 ~KalmanFilter(); 00034 00035 // Facilitators 00036 00037 // Mutators 00038 00039 /*! Executes the state propagation function */ 00040 virtual void PropagateState(); 00041 00042 /*! Executes the state covariance propagation function */ 00043 void PropagateCovariance(); 00044 00045 /*! Executes the Kalman gain calculation function */ 00046 void CalculateKalmanGain(); 00047 00048 /*! Executes the state measurement update function */ 00049 void MeasurementUpdateState(); 00050 00051 /*! Executes the state covariance measurement update function */ 00052 void MeasurementUpdateCovariance(); 00053 00054 /*! Sets the state covariance matrix */ 00055 inline void SetCovarianceMatrix(Matrix& _covariance) { m_covariance = _covariance; }; 00056 00057 /*! Sets the Kalman gain matrix */ 00058 inline void SetKalmanGainMatrix(Matrix& _kalmanGain) { m_kalmanGain = _kalmanGain; }; 00059 00060 /*! Sets the pointer to the state propagation function */ 00061 inline void SetStatePropagator(void (*statePropagateFunc)(KalmanFilter*)) { ptr_statePropagteFunc = statePropagateFunc; }; 00062 00063 /*! Sets the pointer to the state covariance propagation function */ 00064 inline void SetCovariancePropagator(void (*covariancePropagateFunc)(KalmanFilter*)) { ptr_covariancePropagateFunc = covariancePropagateFunc; }; 00065 00066 /*! Sets the pointer to the Kalman gain calculation function */ 00067 inline void SetKalmanGainCalculation(void (*kalmanGainCalculationFunc)(KalmanFilter*)) { ptr_kalmanGainCalculationFunc = kalmanGainCalculationFunc; }; 00068 00069 /*! Sets the pointer to the state measurement update function */ 00070 inline void SetStateMeasurementUpdate(void (*stateMeasurementUpdateFunc)(KalmanFilter*)) { ptr_stateMeasurementUpdateFunc = stateMeasurementUpdateFunc; }; 00071 00072 /*! Sets the pointer to the state covariance measurement update function */ 00073 inline void SetCovarianceMeasurementUpdate(void (*covarianceMeasurementUpdateFunc)(KalmanFilter*)) { ptr_covarianceMeasurementUpdateFunc = covarianceMeasurementUpdateFunc; }; 00074 00075 /*! Sets the pointer to the system process noise matrix function */ 00076 inline void SetSystemProcessNoiseMatrix(Matrix (*systemProcessNoiseMatrix)(KalmanFilter*)) { ptr_systemProcessNoiseMatrix = systemProcessNoiseMatrix; }; 00077 00078 /*! Sets the pointer to the measurement covariance matrix function */ 00079 inline void SetMeasurementCovarianceMatrix(Matrix (*measurementCovarianceMatrix)(KalmanFilter*)) { ptr_measurementCovarianceMatrix = measurementCovarianceMatrix; }; 00080 00081 /*! Sets the pointer to the state Jacobian matrix function */ 00082 inline void SetStateJacobianMatrix(Matrix (*stateJacobianMatrix)(KalmanFilter*)) { ptr_stateJacobianMatrix = stateJacobianMatrix; }; 00083 00084 /*! Sets the pointer to the measurement Jacobian matrix function */ 00085 inline void SetMeasurementJacobianMatrix(Matrix (*measurementJacobianMatrix)(KalmanFilter*)) { ptr_measurementJacobianMatrix = measurementJacobianMatrix; }; 00086 00087 /*! Estimate the state using the last state estimate and the current measurements */ 00088 virtual void EstimateState(); 00089 00090 // Inspectors 00091 00092 /*! Gets the current state covairance matrix */ 00093 Matrix GetCovarianceMatrix(); 00094 00095 /*! Gets the current Kalman gain matrix */ 00096 Matrix GetKalmanGainMatrix(); 00097 00098 /*! Gets the current system process noise matrix */ 00099 Matrix GetSystemProcessNoiseMatrix(); 00100 00101 /*! Gets the current measurement covairance matrix */ 00102 Matrix GetMeasurementCovarianceMatrix(); 00103 00104 /*! Gets the current state Jacobian matrix */ 00105 Matrix GetStateJacobianMatrix(); 00106 00107 /*! Gets the current measurement Jacobian matrix */ 00108 Matrix GetMeasurementJacobianMatrix(); 00109 00110 /*! Gets the current measurement Jacobian matrix */ 00111 virtual inline Vector GetEstimatedMeasurements() { return (ptr_measurementJacobianMatrix(this) * m_measurements); }; 00112 00113 protected: 00114 // protected data members 00115 Matrix m_covariance; /*!< The covariance matrix, P. */ 00116 Matrix m_kalmanGain; /*!< The Kalman Gain matrix, K. */ 00117 //KalmanFilterHistory* mptr_kalmanFilterHistory; 00118 00119 private: 00120 // private data members 00121 void (*ptr_statePropagteFunc)(KalmanFilter*); /*!< A function pointer to the state propagation function */ 00122 void (*ptr_covariancePropagateFunc)(KalmanFilter*); /*!< A function pointer to the state covariance propagation function */ 00123 void (*ptr_kalmanGainCalculationFunc)(KalmanFilter*); /*!< A function pointer to the Kalman gain calculation function */ 00124 void (*ptr_stateMeasurementUpdateFunc)(KalmanFilter*); /*!< A function pointer to the state measurement update function */ 00125 void (*ptr_covarianceMeasurementUpdateFunc)(KalmanFilter*); /*!< A function pointer to the state covariance measurement update function */ 00126 Matrix (*ptr_systemProcessNoiseMatrix)(KalmanFilter*); /*!< A function pointer to the system process noise matrix,Q. */ 00127 Matrix (*ptr_measurementCovarianceMatrix)(KalmanFilter*); /*!< A function pointer to the measurement covariance matrix, R. */ 00128 Matrix (*ptr_stateJacobianMatrix)(KalmanFilter*); /*!< A function pointer to the state jacobian matrix, F. */ 00129 Matrix (*ptr_measurementJacobianMatrix)(KalmanFilter*); /*!< A function pointer to the measurement jacobian matrix, H. */ 00130 00131 }; 00132 00133 00134 00135 #endif 00136 // Do not change the comments below - they will be added automatically by CVS 00137 /***************************************************************************** 00138 * $Log: KalmanFilter.h,v $ 00139 * Revision 1.18 2005/02/25 18:40:54 cakinli 00140 * Created Makefiles and organized include directives to reduce the number of 00141 * include paths. Reorganized libraries so that there is now one per source 00142 * directory. Each directory is self-contained in terms of its Makefile. 00143 * The local Makefile in each directory includes src/config.mk, which has all 00144 * the definitions and general and pattern rules. So at most, to see what 00145 * goes into building a target, a person needs to examine the Makefile in 00146 * that directory, and ../config.mk. 00147 * 00148 * Revision 1.17 2003/07/01 20:49:41 mavandyk 00149 * Changed the private member functions to public member functions so the updates could be done at different time intervals. 00150 * 00151 * Revision 1.16 2003/07/01 19:32:43 simpliciter 00152 * Destructor shouldn't be virtual for a non-abstract class. 00153 * 00154 * Revision 1.15 2003/06/27 22:40:40 mavandyk 00155 * Corrected to reflect changes in EKF stuff. 00156 * 00157 * Revision 1.14 2003/06/24 22:40:07 mavandyk 00158 * Moved inline function definition from implementation to header file, and added comments. 00159 * 00160 * Revision 1.13 2003/06/24 15:40:31 simpliciter 00161 * In brief: I removed the kfData struct. 00162 * That change required numerous other changes. 00163 * Most significant is that functions which previously 00164 * took kfData or sfData as an input now take a KalmanFilter*. 00165 * 00166 * Revision 1.12 2003/06/20 21:58:54 mavandyk 00167 * Pulled pointer types from kalmanFilterData struct and put them directly in the KalmanFilter class. 00168 * 00169 * Revision 1.11 2003/06/20 18:12:17 simpliciter 00170 * Readded inlines for all but GetKalmanFilterData, which doesn't want to be inline. ?? 00171 * 00172 * Revision 1.10 2003/06/20 17:54:57 simpliciter 00173 * Fixed what I broke--need to reinstate inline functions. 00174 * 00175 * Revision 1.9 2003/06/20 17:48:52 simpliciter 00176 * Reincluded inline statements. 00177 * 00178 * Revision 1.8 2003/06/20 15:21:27 simpliciter 00179 * Added deconstructor. 00180 * 00181 * Revision 1.7 2003/06/20 00:03:35 simpliciter 00182 * Fixed function pointer calls. 00183 * 00184 * Revision 1.6 2003/06/18 17:37:34 mavandyk 00185 * Added include line for KalmanFilter.h and changed the function pointers in the kalmanFilterData to take in type sequentialFilterData. 00186 * 00187 * Revision 1.5 2003/06/12 16:10:36 mavandyk 00188 * Changed all references to iterative filter to sequential filter for greater clarity. 00189 * 00190 * Revision 1.4 2003/06/11 17:52:26 simpliciter 00191 * Made a derived class from IterativeFilter. 00192 * 00193 * Revision 1.3 2003/06/11 15:45:32 simpliciter 00194 * Added IterativeFilter.h include line. 00195 * 00196 * Revision 1.2 2003/06/11 13:07:50 simpliciter 00197 * Minor changes. 00198 * 00199 * Revision 1.1 2003/06/11 03:03:46 simpliciter 00200 * Initial submission. 00201 * 00202 ******************************************************************************/
1.3.9.1