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 #include "cxcore.h"
29 
36 namespace alvar {
37 
38  class UnscentedProcess;
39  class UnscentedObservation;
40 
97  private:
98  int state_n;
99  int state_k;
100  int obs_n;
101  int sigma_n;
103  double lambda, lambda2;
104 
105  CvMat *state;
108  CvMat *stateD;
109  CvMat *stateU;
110  CvMat *stateV;
111  CvMat *stateTmp;
112  CvMat *stateDiff;
113 
114  CvMat *predObs;
117  CvMat *predObsDiff;
118 
120  CvMat *kalmanGain;
121  CvMat *kalmanTmp;
122 
123  CvMat **sigma_state;
124  CvMat **sigma_predObs;
125 
126  // possess state mean and co-variance (as a list of sigma points).
127  // generate sigma points from state mean vector and co-variance matrix.
128  // compute state mean vector and co-variance matrix from sigma points.
129 
130  // predict:
131  // - map sigma points thru process model f.
132 
133  // update:
134  // - map sigma points thru h.
135  // - from current sigma points and sigma observations:
136  // - compute state estimate x and co-variance P.
137  // - compute predicted observation z and innocation co-variance Z
138  // - compute cross correlation XZ
139  // - compute new state mean and co-variance.
140  // - generate new sigma points.
141  public:
142 
160  UnscentedKalman(int state_n, int obs_n, int state_k = 0, double alpha = 0.001, double beta = 2.0);
161  ~UnscentedKalman();
162 
173  CvMat *getState() { return state; }
174 
184  CvMat *getStateCovariance() { return stateCovariance; }
185 
191  void initialize();
192 
201  void predict(UnscentedProcess *process_model);
202 
213  void update(UnscentedObservation *observation);
214  };
215 
222  public:
231  virtual void f(CvMat *state) = 0;
232 
242  virtual CvMat *getProcessNoise() = 0;
243  };
244 
253  public:
261  virtual void h(CvMat *z, CvMat *state) = 0;
262 
272  virtual CvMat *getObservation() = 0;
273 
284  virtual CvMat *getObservationNoise() = 0;
285  };
286 
287 } // namespace alvar
288 
289 #endif // __UNSCENTED_KALMAN__
Main ALVAR namespace.
Definition: Alvar.h:174
CvMat * getState()
Returns the process state vector.
f
ROSCONSOLE_DECL void initialize()
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
TFSIMD_FORCE_INLINE const tfScalar & z() const
#define ALVAR_EXPORT
Definition: Alvar.h:168
Observation model for an unscented kalman filter.
CvMat * getStateCovariance()
Returns the process state covariance matrix.
This file defines library export definitions, version numbers and build information.
Process model for an unscented kalman filter.
Implementation of unscented kalman filter (UKF) for filtering non-linear processes.


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Jun 6 2019 19:27:24