My Project
UnscentedKalman.h
Go to the documentation of this file.
1 /*
2  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
3  *
4  * Copyright 2007-2012 VTT Technical Research Centre of Finland
5  *
6  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
7  * <http://www.vtt.fi/multimedia/alvar.html>
8  *
9  * ALVAR is free software; you can redistribute it and/or modify it under the
10  * terms of the GNU Lesser General Public License as published by the Free
11  * Software Foundation; either version 2.1 of the License, or (at your option)
12  * any later version.
13  *
14  * This library is distributed in the hope that it will be useful, but WITHOUT
15  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
16  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
17  * for more details.
18  *
19  * You should have received a copy of the GNU Lesser General Public License
20  * along with ALVAR; if not, see
21  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
22  */
23 
24 #ifndef __UNSCENTED_KALMAN__
25 #define __UNSCENTED_KALMAN__
26 
27 #include "Alvar.h"
28 
29 #include <opencv2/core.hpp>
30 
37 namespace alvar {
38 
39 class UnscentedProcess;
40 class UnscentedObservation;
41 
97 class ALVAR_EXPORT UnscentedKalman
98 {
99 private:
100  int state_n;
101  int state_k;
102  int obs_n;
103  int sigma_n;
104  bool sigmasUpdated;
105  double lambda, lambda2;
106 
107  cv::Mat state;
108  cv::Mat stateCovariance;
109  cv::Mat sqrtStateCovariance;
110  cv::Mat stateD;
111  cv::Mat stateU;
112  cv::Mat stateV;
113  cv::Mat stateTmp;
114  cv::Mat stateDiff;
115 
116  cv::Mat predObs;
117  cv::Mat predObsCovariance;
118  cv::Mat invPredObsCovariance;
119  cv::Mat predObsDiff;
120 
121  cv::Mat statePredObsCrossCorrelation;
122  cv::Mat kalmanGain;
123  cv::Mat kalmanTmp;
124 
125  std::vector<cv::Mat> sigma_state;
126  std::vector<cv::Mat> sigma_predObs;
127 
128  // possess state mean and co-variance (as a list of sigma points).
129  // generate sigma points from state mean vector and co-variance matrix.
130  // compute state mean vector and co-variance matrix from sigma points.
131 
132  // predict:
133  // - map sigma points thru process model f.
134 
135  // update:
136  // - map sigma points thru h.
137  // - from current sigma points and sigma observations:
138  // - compute state estimate x and co-variance P.
139  // - compute predicted observation z and innocation co-variance Z
140  // - compute cross correlation XZ
141  // - compute new state mean and co-variance.
142  // - generate new sigma points.
143 public:
161  UnscentedKalman(int state_n, int obs_n, int state_k = 0, double alpha = 0.001, double beta = 2.0);
162  ~UnscentedKalman();
163 
174  cv::Mat
176  {
177  return state;
178  }
179 
189  cv::Mat
191  {
192  return stateCovariance;
193  }
194 
200  void initialize();
201 
210  void predict(UnscentedProcess *process_model);
211 
222  void update(UnscentedObservation *observation);
223 };
224 
230 class ALVAR_EXPORT UnscentedProcess
231 {
232 public:
241  virtual void f(cv::Mat &state) = 0;
242 
252  virtual cv::Mat getProcessNoise() = 0;
253 };
254 
262 class ALVAR_EXPORT UnscentedObservation
263 {
264 public:
272  virtual void h(cv::Mat &z, cv::Mat &state) = 0;
273 
283  virtual cv::Mat getObservation() = 0;
284 
295  virtual cv::Mat getObservationNoise() = 0;
296 };
297 
298 } // namespace alvar
299 
300 #endif // __UNSCENTED_KALMAN__
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.
Main ALVAR namespace.
Definition: Alvar.h:174