@@ -30,74 +30,114 @@ namespace ser94mor
3030 {
3131
3232 /* *
33- * A template class holding Kalman Filter equations.
33+ * A base template class holding Kalman filter equations.
3434 * The naming of vectors and matrices are taken from the
3535 * "Thrun, S., Burgard, W. and Fox, D., 2005. Probabilistic robotics. MIT press."
3636 *
3737 * @tparam ProcessModel a class of the process model to use
3838 * @tparam MeasurementModel a class of measurement model to use; notice that here it is not a template class
39+ * @tparam DerivedKalmanFilter a concrete sub-template-class of the KalmanFilterBase;
40+ * it is needed to invoke methods from that class
3941 */
40- template <class ProcessModel , class MeasurementModel >
41- class KalmanFilter
42+ template <class ProcessModel , class MeasurementModel , template < class , class > class DerivedKalmanFilter >
43+ class KalmanFilterBase
4244 {
4345 protected:
4446 using Belief = typename ProcessModel::Belief_type;
4547 using ControlVector = typename ProcessModel::ControlVector_type;
4648 using Measurement = typename MeasurementModel::Measurement_type;
47-
4849 public:
50+
4951 /* *
5052 * Prediction step of the Kalman filter. Predicts the object's state in dt time in the future in accordance with
5153 * LINEAR process model and input control vector.
5254 *
53- * @param belief_posterior a current belief of the object's state
55+ * @param bel a current belief of the object's state
5456 * @param ut a control vector
5557 * @param dt time interval between the previous and current measurements
5658 * @param process_model an instance of the process model
5759 *
5860 * @return a prior belief, that is, after prediction but before incorporating the measurement
5961 */
60- template <bool EnableBool = true >
61- static Belief Predict (const Belief& belief_posterior,
62- const ControlVector& ut,
63- double dt,
64- const std::
enable_if_t <ProcessModel::IsLinear() && EnableBool, ProcessModel>& process_model)
62+ template <bool enable = true >
63+ static auto
64+ Predict (const Belief& bel, const ControlVector& ut, double_t dt, const ProcessModel& process_model)
65+ -> std::
enable_if_t <ProcessModel::IsLinear()
and enable, Belief>
6566 {
6667 auto At{process_model.A (dt)};
6768 return {
68- /* timestamp */ belief_posterior .t () + dt,
69- /* state vector */ belief_posterior .mu () + process_model.B () * ut,
70- /* state covariance matrix */ belief_posterior .Sigma () * At.transpose () + process_model.R (dt),
69+ /* timestamp */ bel .t () + dt,
70+ /* state vector */ bel .mu () + process_model.B () * ut,
71+ /* state covariance matrix */ bel .Sigma () * At.transpose () + process_model.R (dt),
7172 };
7273 }
7374
7475 /* *
7576 * Update step of the Kalman filter. Incorporates the sensor measurement into the given prior belief.
7677 * Works only with linear measurement models.
7778 *
78- * @param belief_prior a belief after the prediction Kalman filter step
79+ * @param bel a belief after the prediction Kalman filter step
7980 * @param measurement a measurement from the sensor
8081 * @param measurement_model an instance of the measurement model
8182 *
8283 * @return a posterior belief, that is, after the incorporation of the measurement
8384 */
84- template <bool EnableBool = true >
85- static Belief Update ( const Belief& belief_prior, const Measurement& measurement,
86- const std:: enable_if_t <MeasurementModel::IsLinear() && EnableBool, MeasurementModel>&
87- measurement_model)
85+ template <bool enable = true >
86+ static auto
87+ Update ( const Belief& bel, const Measurement& measurement, const MeasurementModel& measurement_model)
88+ -> std:: enable_if_t <MeasurementModel::IsLinear() and enable, Belief>
8889 {
8990 auto Ct{measurement_model.C ()};
90- auto mu{belief_prior .mu ()};
91- auto Sigma{belief_prior .Sigma ()};
91+ auto mu{bel .mu ()};
92+ auto Sigma{bel .Sigma ()};
9293 auto Kt{Sigma * Ct.transpose () * (Ct * Sigma * Ct.transpose () + measurement_model.Q ()).inverse ()};
93- auto I{Eigen::Matrix<double , ProcessModel::StateDims (), ProcessModel::StateDims ()>::Identity ()};
94+ auto I{Eigen::Matrix<double_t , ProcessModel::StateDims (), ProcessModel::StateDims ()>::Identity ()};
9495
9596 return {
9697 /* timestamp */ t (),
9798 /* state vector */ z () - Ct * mu),
9899 /* state covariance matrix */
99100 };
100101 }
102+
103+ /* *
104+ * Combined Predict and Update steps of the derived Kalman filter.
105+ * Predicts the object's state in dt time in the future in accordance with the process model
106+ * and input control vector and then incorporates the sensor measurement into the belief.
107+ *
108+ * Notice that process model and mesurement model can be either linear and non-linear.
109+ *
110+ * @param bel a current belief of the object's state
111+ * @param ut a control vector
112+ * @param measurement a measurement from the sensor
113+ * @param process_model an instance of the process model
114+ * @param measurement_model an instance of the measurement model
115+ * @return a posterior belief, that is, after the prediction and incorporation of the measurement
116+ */
117+ static Belief
118+ PredictUpdate (const Belief& bel, const ControlVector& ut, const Measurement& measurement,
119+ const ProcessModel& process_model, const MeasurementModel& measurement_model)
120+ {
121+ auto dt = measurement.t () - bel.t ();
122+
123+ auto belief_prior{DerivedKalmanFilter<ProcessModel, MeasurementModel>::
Predict (bel, ut, dt, process_model)};
124+
125+ return DerivedKalmanFilter<ProcessModel, MeasurementModel>::
126+ Update (belief_prior, measurement, measurement_model);
127+ }
128+ };
129+
130+
131+ /* *
132+ * A concrete class representing Kalman filter. All the equations are the same as in its base class.
133+ *
134+ * @tparam ProcessModel a class of the process model to use
135+ * @tparam MeasurementModel a class of measurement model to use; notice that here it is not a template class
136+ */
137+ template <class ProcessModel , class MeasurementModel >
138+ class KalmanFilter :
public KalmanFilterBase <ProcessModel, MeasurementModel, KalmanFilter>
139+ {
140+
101141 };
102142
103143 }
0 commit comments