UnscentedKalman.cpp
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 #include "UnscentedKalman.h"
25 #include <cxcore.h>
26 #include <stdio.h>
27 
28 namespace alvar {
29 
30 UnscentedKalman::UnscentedKalman(int state_n, int obs_n, int state_k, double alpha, double beta) {
31  state_k = 0;
32  //TODO: support a separate noise vector/covariance matrix: state_k;
33  this->state_k = state_k;
34  this->state_n = state_n;
35  this->obs_n = obs_n;
36  sigma_n = 2 * state_n + 1;
37 
38  double L = state_n + state_k;
39  lambda = alpha*alpha * L - L;
40  lambda2 = 1 - alpha*alpha + beta;
41 
42  state = cvCreateMat(state_n, 1, CV_64F); cvSetZero(state);
43  stateCovariance = cvCreateMat(state_n, state_n, CV_64F); cvSetZero(stateCovariance);
44  sqrtStateCovariance = cvCreateMat(state_n, state_n, CV_64F); cvSetZero(sqrtStateCovariance);
45  stateD = cvCreateMat(state_n, state_n, CV_64F); cvSetZero(stateD);
46  stateU = cvCreateMat(state_n, state_n, CV_64F); cvSetZero(stateU);
47  stateV = cvCreateMat(state_n, state_n, CV_64F); cvSetZero(stateV);
48  stateTmp = cvCreateMat(state_n, state_n, CV_64F); cvSetZero(stateTmp);
49  stateDiff = cvCreateMat(state_n, 1, CV_64F); cvSetZero(stateDiff);
50 
51  predObs = cvCreateMat(obs_n, 1, CV_64F); cvSetZero(predObs);
52  predObsCovariance = cvCreateMat(obs_n, obs_n, CV_64F); cvSetZero(predObsCovariance);
53  predObsDiff = cvCreateMat(obs_n, 1, CV_64F); cvSetZero(predObsDiff);
54 
55  invPredObsCovariance = cvCreateMat(obs_n, obs_n, CV_64F); cvSetZero(invPredObsCovariance);
56  statePredObsCrossCorrelation = cvCreateMat(state_n, obs_n, CV_64F); cvSetZero(statePredObsCrossCorrelation);
57  kalmanGain = cvCreateMat(state_n, obs_n, CV_64F); cvSetZero(kalmanGain);
58  kalmanTmp = cvCreateMat(state_n, obs_n, CV_64F); cvSetZero(kalmanTmp);
59 
60  sigma_state = new CvMat*[sigma_n];
61  sigma_predObs = new CvMat*[sigma_n];
62 
63  for (int i = 0; i < sigma_n; i++) {
64  sigma_state[i] = cvCreateMat(state_n, 1, CV_64F); cvSetZero(sigma_state[i]);
65  sigma_predObs[i] = cvCreateMat(obs_n, 1, CV_64F); cvSetZero(sigma_predObs[i]);
66  }
67 
68  sigmasUpdated = false;
69 }
70 
72  cvReleaseMat(&state);
73  cvReleaseMat(&stateCovariance);
74  cvReleaseMat(&sqrtStateCovariance);
75  cvReleaseMat(&stateD);
76  cvReleaseMat(&stateU);
77  cvReleaseMat(&stateV);
78  cvReleaseMat(&stateTmp);
79  cvReleaseMat(&stateDiff);
80  cvReleaseMat(&kalmanTmp);
81  cvReleaseMat(&kalmanGain);
82  cvReleaseMat(&statePredObsCrossCorrelation);
83  cvReleaseMat(&invPredObsCovariance);
84 
85  cvReleaseMat(&predObs);
86  cvReleaseMat(&predObsCovariance);
87  cvReleaseMat(&predObsDiff);
88 
89  for (int i = 0; i < sigma_n; i++) {
90  cvReleaseMat(&sigma_state[i]);
91  cvReleaseMat(&sigma_predObs[i]);
92  }
93  delete[] sigma_state;
94  delete[] sigma_predObs;
95 }
96 
98  // Computes new sigma points from current state estimate.
99 
100  // 1. Compute square root of state co-variance:
101  //[E D] = eig(A); sqrtm(A) = E * sqrt(D) * E' where D is a diagonal matrix.
102  //sqrt(D) is formed by taking the square root of the diagonal entries in D.
103  #ifdef MYDEBUG
104  printf("stateCovariance:\n");
105  for (int i = 0; i < 5; i++)
106  printf("%+.10f %+.10f %+.10f %+.10f %+.10f\n",
107  cvGetReal2D(stateCovariance, 0, i),
108  cvGetReal2D(stateCovariance, 1, i),
109  cvGetReal2D(stateCovariance, 2, i),
110  cvGetReal2D(stateCovariance, 3, i),
111  cvGetReal2D(stateCovariance, 4, i));
112 #endif
113 
114  //Another equivilant way is to use:
115  // [U S V] = svd(A); sqrtm(A) = U * sqrt(S) * V'
116  cvSVD(stateCovariance, stateD, stateU, stateV); //, CV_SVD_V_T
117  double L = state_n + state_k;
118  double scale = L + lambda;
119  for (int i = 0; i < state_n; i++) {
120  double d = cvGetReal2D(stateD, i, i);
121  cvSetReal2D(stateD, i, i, sqrt(scale*d));
122  }
123  cvGEMM(stateD, stateV, 1., NULL, 0, stateTmp, CV_GEMM_B_T);
124  cvGEMM(stateU, stateTmp, 1., NULL, 0, sqrtStateCovariance);
125 #ifdef MYDEBUG
126  printf("sqrtStateCovariance:\n");
127  for (int i = 0; i < 5; i++)
128  printf("%+.10f %+.10f %+.10f %+.10f %+.10f\n",
129  cvGetReal2D(sqrtStateCovariance, 0, i),
130  cvGetReal2D(sqrtStateCovariance, 1, i),
131  cvGetReal2D(sqrtStateCovariance, 2, i),
132  cvGetReal2D(sqrtStateCovariance, 3, i),
133  cvGetReal2D(sqrtStateCovariance, 4, i));
134  cvGEMM(sqrtStateCovariance, sqrtStateCovariance, 1., NULL, 0, stateTmp);
135  printf("sqrtStateCovariance^2:\n");
136  for (int i = 0; i < 5; i++)
137  printf("%+.10f %+.10f %+.10f %+.10f %+.10f\n",
138  cvGetReal2D(stateTmp, 0, i),
139  cvGetReal2D(stateTmp, 1, i),
140  cvGetReal2D(stateTmp, 2, i),
141  cvGetReal2D(stateTmp, 3, i),
142  cvGetReal2D(stateTmp, 4, i));
143 #endif
144 
145  // 2. Form new sigma points.
146  int sigma_i = 0;
147  cvCopy(state, sigma_state[sigma_i++]);
148  for (int i = 0; i < state_n; i++) {
149  CvMat col;
150  cvGetCol(sqrtStateCovariance, &col, i);
151  cvAdd(state, &col, sigma_state[sigma_i++]);
152  cvSub(state, &col, sigma_state[sigma_i++]);
153  }
154 
155  sigmasUpdated = true;
156 }
157 
159  if (!sigmasUpdated) initialize();
160 
161  // Map sigma points through the process model and compute new state mean.
162  cvSetZero(state);
163  double L = state_n + state_k;
164  double totalWeight = 0;
165  for (int i = 0; i < sigma_n; i++) {
166  double weight = i == 0
167  ? lambda / (L + lambda)
168  : .5 / (L + lambda);
169  totalWeight += weight;
170  }
171  for (int i = 0; i < sigma_n; i++) {
172  CvMat *sigma = sigma_state[i];
173  process_model->f(sigma);
174  double weight = i == 0
175  ? lambda / (L + lambda)
176  : .5 / (L + lambda);
177  double scale = weight / totalWeight;
178  cvAddWeighted(sigma, scale, state, 1., 0., state);
179  }
180 
181  // Compute new state co-variance.
182  cvSetZero(stateCovariance);
183  totalWeight = 0;
184  for (int i = 0; i < sigma_n; i++) {
185  double weight = i == 0
186  ? lambda / (L + lambda) + lambda2
187  : .5 / (L + lambda);
188  totalWeight += weight;
189  }
190  for (int i = 0; i < sigma_n; i++) {
191  double weight = i == 0
192  ? lambda / (L + lambda) + lambda2
193  : .5 / (L + lambda);
194  double scale = weight / totalWeight;
195  cvSub(sigma_state[i], state, stateDiff);
196  cvGEMM(stateDiff, stateDiff, scale, stateCovariance, 1., stateCovariance,
197  CV_GEMM_B_T);
198  }
199 
200  // Add any additive noise.
201  CvMat *noise = process_model->getProcessNoise();
202  if (noise) cvAdd(stateCovariance, noise, stateCovariance);
203 
204 #ifdef MYDEBUG
205  printf("predicted state: ");
206  for (int i = 0; i < state_n; i++) printf("%f ", cvGetReal1D(state, i));
207  printf("\n");
208  printf("predicted stateCovariance:\n");
209  for (int i = 0; i < state_n; i++) {
210  for (int j = 0; j < state_n; j++) printf("%+f ", cvGetReal2D(stateCovariance, i, j));
211  printf("\n");
212  }
213 #endif
214 
215  sigmasUpdated = false;
216 }
217 
219  if (!sigmasUpdated) initialize();
220  CvMat *innovation = obs->getObservation();
221  int obs_n = innovation->rows;
222  if (obs_n > this->obs_n) {
223  printf("Observation exceeds maximum size!\n");
224  abort();
225  }
226 
227  // Map sigma points through the observation model and compute predicted mean.
228  CvMat predObs = cvMat(obs_n, 1, CV_64F, this->predObs->data.db);
229  cvSetZero(&predObs);
230  for (int i = 0; i < sigma_n; i++) {
231  CvMat sigma_h = cvMat(obs_n, 1, CV_64F, sigma_predObs[i]->data.db);
232  double scale = i == 0
233  ? (double)state_k / (double)(state_n + state_k)
234  : .5 / (double)(state_n + state_k);
235  obs->h(&sigma_h, sigma_state[i]);
236  cvAddWeighted(&sigma_h, scale, &predObs, 1., 0., &predObs);
237  }
238 
239  // Compute predicted observation co-variance.
240  CvMat predObsCovariance = cvMat(obs_n, obs_n, CV_64F,
241  this->predObsCovariance->data.db);
242  CvMat statePredObsCrossCorrelation = cvMat(state_n, obs_n, CV_64F,
243  this->statePredObsCrossCorrelation->data.db);
244  CvMat predObsDiff = cvMat(obs_n, 1, CV_64F, this->predObsDiff->data.db);
245  cvSetZero(&predObsCovariance);
246  cvSetZero(&statePredObsCrossCorrelation);
247  for (int i = 0; i < sigma_n; i++) {
248  CvMat sigma_h = cvMat(obs_n, 1, CV_64F, sigma_predObs[i]->data.db);
249  double scale = i == 0
250  ? (double)state_k / (double)(state_n + state_k)
251  : .5 / (double)(state_n + state_k);
252  cvSub(sigma_state[i], state, stateDiff);
253  cvSub(&sigma_h, &predObs, &predObsDiff);
254  cvGEMM(&predObsDiff, &predObsDiff, scale, &predObsCovariance, 1., &predObsCovariance,
255  CV_GEMM_B_T);
256  cvGEMM(stateDiff, &predObsDiff, scale, &statePredObsCrossCorrelation, 1.,
257  &statePredObsCrossCorrelation, CV_GEMM_B_T);
258  }
259 
260  // Add any additive noise.
261  CvMat *noise = obs->getObservationNoise();
262  if (noise) cvAdd(&predObsCovariance, noise, &predObsCovariance);
263 
264 #ifdef MYDEBUG
265  printf("real observation: ");
266  for (int i = 0; i < obs_n; i++) printf("%+f ", cvGetReal1D(innovation ,i));
267  printf("\n");
268  printf("predicted observation: ");
269  for (int i = 0; i < obs_n; i++) printf("%+f ", cvGetReal1D(&predObs,i));
270  printf("\n");
271  printf("predicted observation co-variance\n");
272  for (int i = 0; i < obs_n; i++) {
273  for (int j = 0; j < obs_n; j++) printf("%+f ", cvGetReal2D(&predObsCovariance,i,j));
274  printf("\n");
275  }
276  printf("state observation cross-correlation\n");
277  for (int i = 0; i < state_n; i++) {
278  for (int j = 0; j < obs_n; j++) printf("%+f ", cvGetReal2D(&statePredObsCrossCorrelation,i,j));
279  printf("\n");
280  }
281 #endif
282 
283  // Update state mean and co-variance.
284  // innovation: v = z - pz
285  // gain: W = XZ * (R + Z)^-1
286  // state: x = x + _W * v
287  // co-var: P = P - W * (R + Z) * W^T
288 
289  CvMat invPredObsCovariance = cvMat(obs_n, obs_n, CV_64F,
290  this->invPredObsCovariance->data.db);
291  CvMat kalmanGain = cvMat(state_n, obs_n, CV_64F, this->kalmanGain->data.db);
292  CvMat kalmanTmp = cvMat(state_n, obs_n, CV_64F, this->kalmanTmp->data.db);
293 
294  cvSub(innovation, &predObs, innovation);
295  //double inno_norm = cvNorm(innovation) / obs_n;
296  //if (inno_norm > 5.0) {
297  // return;
298  //}
299 
300 #ifdef MYDEBUG
301  printf("innovation: ");
302  for (int i = 0; i < obs_n; i++) printf("%f ", cvGetReal1D(innovation,i));
303  printf("\n");
304  double inn_norm = cvNorm(innovation);
305  printf("innivation norm: %f\n", inn_norm);
306 #endif
307 
308  cvInvert(&predObsCovariance, &invPredObsCovariance, CV_SVD_SYM);
309  cvMatMul(&statePredObsCrossCorrelation, &invPredObsCovariance, &kalmanGain);
310  cvGEMM(&kalmanGain, innovation, 1., state, 1., state);
311  cvMatMul(&kalmanGain, &predObsCovariance, &kalmanTmp);
312  cvGEMM(&kalmanTmp, &kalmanGain, -1., stateCovariance, 1., stateCovariance,
313  CV_GEMM_B_T);
314 #ifdef MYDEBUG
315  printf("estimated state: ");
316  for (int i = 0; i < state_n; i++) printf("%f ", cvGetReal1D(state, i));
317  printf("\n");
318 #endif
319 
320  sigmasUpdated = false;
321 }
322 
323 } // namespace alvar
Main ALVAR namespace.
Definition: Alvar.h:174
This file implements an unscented Kalman filter.
void update(UnscentedObservation *observation)
Updates the state by an observation.
virtual void h(CvMat *z, CvMat *state)=0
observation model: z = h(state)
virtual CvMat * getObservationNoise()=0
Returns the observation noise covariance matrix.
virtual void f(CvMat *state)=0
process model: state+1 = f(state)
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.
void predict(UnscentedProcess *process_model)
Updated the state by predicting.
Drawable d[32]
Observation model for an unscented kalman filter.
virtual CvMat * getProcessNoise()=0
Returns the process noise covariance.
virtual CvMat * getObservation()=0
Returns the current measurement vector.
Process model for an unscented kalman filter.


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