24 #ifndef __UNSCENTED_KALMAN__
25 #define __UNSCENTED_KALMAN__
29 #include <opencv2/core.hpp>
39 class UnscentedProcess;
40 class UnscentedObservation;
105 double lambda, lambda2;
108 cv::Mat stateCovariance;
109 cv::Mat sqrtStateCovariance;
117 cv::Mat predObsCovariance;
118 cv::Mat invPredObsCovariance;
121 cv::Mat statePredObsCrossCorrelation;
125 std::vector<cv::Mat> sigma_state;
126 std::vector<cv::Mat> sigma_predObs;
161 UnscentedKalman(
int state_n,
int obs_n,
int state_k = 0,
double alpha = 0.001,
double beta = 2.0);
192 return stateCovariance;
241 virtual void f(cv::Mat &state) = 0;
272 virtual void h(cv::Mat &z, cv::Mat &state) = 0;
This file defines library export definitions, version numbers and build information.
Implementation of unscented kalman filter (UKF) for filtering non-linear processes.
cv::Mat getStateCovariance()
Returns the process state covariance matrix.
UnscentedKalman(int state_n, int obs_n, int state_k=0, double alpha=0.001, double beta=2.0)
Initializes Unscented Kalman filter.
void initialize()
(Re-)initialize UKF internal state.
cv::Mat getState()
Returns the process state vector.
void update(UnscentedObservation *observation)
Updates the state by an observation.
void predict(UnscentedProcess *process_model)
Updated the state by predicting.
Observation model for an unscented kalman filter.
virtual cv::Mat getObservationNoise()=0
Returns the observation noise covariance matrix.
virtual void h(cv::Mat &z, cv::Mat &state)=0
observation model: z = h(state)
virtual cv::Mat getObservation()=0
Returns the current measurement vector.
Process model for an unscented kalman filter.
virtual void f(cv::Mat &state)=0
process model: state+1 = f(state)
virtual cv::Mat getProcessNoise()=0
Returns the process noise covariance.