UnscentedKalman.cpp
Go to the documentation of this file.
00001 /*
00002  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
00003  *
00004  * Copyright 2007-2012 VTT Technical Research Centre of Finland
00005  *
00006  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
00007  *          <http://www.vtt.fi/multimedia/alvar.html>
00008  *
00009  * ALVAR is free software; you can redistribute it and/or modify it under the
00010  * terms of the GNU Lesser General Public License as published by the Free
00011  * Software Foundation; either version 2.1 of the License, or (at your option)
00012  * any later version.
00013  *
00014  * This library is distributed in the hope that it will be useful, but WITHOUT
00015  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00016  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
00017  * for more details.
00018  *
00019  * You should have received a copy of the GNU Lesser General Public License
00020  * along with ALVAR; if not, see
00021  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
00022  */
00023 
00024 #include "UnscentedKalman.h"
00025 #include <cxcore.h>
00026 #include <stdio.h>
00027 
00028 namespace alvar {
00029 
00030 UnscentedKalman::UnscentedKalman(int state_n, int obs_n, int state_k, double alpha, double beta) {
00031   state_k = 0;
00032   //TODO: support a separate noise vector/covariance matrix: state_k;
00033   this->state_k = state_k;
00034   this->state_n = state_n;
00035   this->obs_n = obs_n;
00036   sigma_n = 2 * state_n + 1;
00037 
00038   double L = state_n + state_k;
00039   lambda = alpha*alpha * L - L;
00040   lambda2 = 1 - alpha*alpha + beta;
00041 
00042   state = cvCreateMat(state_n, 1, CV_64F); cvSetZero(state);
00043   stateCovariance = cvCreateMat(state_n, state_n, CV_64F); cvSetZero(stateCovariance);
00044   sqrtStateCovariance = cvCreateMat(state_n, state_n, CV_64F); cvSetZero(sqrtStateCovariance);
00045   stateD = cvCreateMat(state_n, state_n, CV_64F); cvSetZero(stateD);
00046   stateU = cvCreateMat(state_n, state_n, CV_64F); cvSetZero(stateU);
00047   stateV = cvCreateMat(state_n, state_n, CV_64F); cvSetZero(stateV);
00048   stateTmp = cvCreateMat(state_n, state_n, CV_64F); cvSetZero(stateTmp);
00049   stateDiff = cvCreateMat(state_n, 1, CV_64F); cvSetZero(stateDiff);
00050 
00051   predObs = cvCreateMat(obs_n, 1, CV_64F); cvSetZero(predObs);
00052   predObsCovariance = cvCreateMat(obs_n, obs_n, CV_64F); cvSetZero(predObsCovariance);
00053   predObsDiff = cvCreateMat(obs_n, 1, CV_64F); cvSetZero(predObsDiff);
00054 
00055   invPredObsCovariance = cvCreateMat(obs_n, obs_n, CV_64F); cvSetZero(invPredObsCovariance);
00056   statePredObsCrossCorrelation = cvCreateMat(state_n, obs_n, CV_64F); cvSetZero(statePredObsCrossCorrelation);
00057   kalmanGain = cvCreateMat(state_n, obs_n, CV_64F); cvSetZero(kalmanGain);
00058   kalmanTmp = cvCreateMat(state_n, obs_n, CV_64F); cvSetZero(kalmanTmp);
00059 
00060   sigma_state = new CvMat*[sigma_n];
00061   sigma_predObs = new CvMat*[sigma_n];
00062 
00063   for (int i = 0; i < sigma_n; i++) {
00064     sigma_state[i] = cvCreateMat(state_n, 1, CV_64F); cvSetZero(sigma_state[i]);
00065     sigma_predObs[i] = cvCreateMat(obs_n, 1, CV_64F); cvSetZero(sigma_predObs[i]);
00066   }
00067 
00068   sigmasUpdated = false;
00069 }
00070 
00071 UnscentedKalman::~UnscentedKalman() {
00072   cvReleaseMat(&state);
00073   cvReleaseMat(&stateCovariance);
00074   cvReleaseMat(&sqrtStateCovariance);
00075   cvReleaseMat(&stateD);
00076   cvReleaseMat(&stateU);
00077   cvReleaseMat(&stateV);
00078   cvReleaseMat(&stateTmp);
00079   cvReleaseMat(&stateDiff);
00080   cvReleaseMat(&kalmanTmp);
00081   cvReleaseMat(&kalmanGain);
00082   cvReleaseMat(&statePredObsCrossCorrelation);
00083   cvReleaseMat(&invPredObsCovariance);
00084 
00085   cvReleaseMat(&predObs);
00086   cvReleaseMat(&predObsCovariance);
00087   cvReleaseMat(&predObsDiff);
00088 
00089   for (int i = 0; i < sigma_n; i++) {
00090     cvReleaseMat(&sigma_state[i]);
00091     cvReleaseMat(&sigma_predObs[i]);
00092   }
00093   delete[] sigma_state;
00094   delete[] sigma_predObs;
00095 }
00096 
00097 void UnscentedKalman::initialize() {
00098   // Computes new sigma points from current state estimate.
00099 
00100   // 1. Compute square root of state co-variance:
00101   //[E D] = eig(A); sqrtm(A) = E * sqrt(D) * E' where D is a diagonal matrix.
00102   //sqrt(D) is formed by taking the square root of the diagonal entries in D.
00103  #ifdef MYDEBUG
00104         printf("stateCovariance:\n");
00105         for (int i = 0; i < 5; i++)
00106           printf("%+.10f %+.10f %+.10f %+.10f %+.10f\n",
00107                  cvGetReal2D(stateCovariance, 0, i),
00108                  cvGetReal2D(stateCovariance, 1, i),
00109                  cvGetReal2D(stateCovariance, 2, i),
00110                  cvGetReal2D(stateCovariance, 3, i),
00111                  cvGetReal2D(stateCovariance, 4, i));
00112 #endif
00113 
00114   //Another equivilant way is to use:
00115   // [U S V] = svd(A); sqrtm(A) = U * sqrt(S) * V'
00116   cvSVD(stateCovariance, stateD, stateU, stateV); //, CV_SVD_V_T
00117   double L = state_n + state_k;
00118   double scale = L + lambda;
00119   for (int i = 0; i < state_n; i++) {
00120     double d = cvGetReal2D(stateD, i, i);
00121     cvSetReal2D(stateD, i, i, sqrt(scale*d));
00122   }
00123   cvGEMM(stateD, stateV, 1., NULL, 0, stateTmp, CV_GEMM_B_T);
00124   cvGEMM(stateU, stateTmp, 1., NULL, 0, sqrtStateCovariance);
00125 #ifdef MYDEBUG
00126         printf("sqrtStateCovariance:\n");
00127         for (int i = 0; i < 5; i++)
00128           printf("%+.10f %+.10f %+.10f %+.10f %+.10f\n",
00129                  cvGetReal2D(sqrtStateCovariance, 0, i),
00130                  cvGetReal2D(sqrtStateCovariance, 1, i),
00131                  cvGetReal2D(sqrtStateCovariance, 2, i),
00132                  cvGetReal2D(sqrtStateCovariance, 3, i),
00133                  cvGetReal2D(sqrtStateCovariance, 4, i));
00134         cvGEMM(sqrtStateCovariance, sqrtStateCovariance, 1., NULL, 0, stateTmp);
00135         printf("sqrtStateCovariance^2:\n");
00136         for (int i = 0; i < 5; i++)
00137           printf("%+.10f %+.10f %+.10f %+.10f %+.10f\n",
00138                  cvGetReal2D(stateTmp, 0, i),
00139                  cvGetReal2D(stateTmp, 1, i),
00140                  cvGetReal2D(stateTmp, 2, i),
00141                  cvGetReal2D(stateTmp, 3, i),
00142                  cvGetReal2D(stateTmp, 4, i));
00143 #endif
00144 
00145   // 2. Form new sigma points.
00146   int sigma_i = 0;
00147   cvCopy(state, sigma_state[sigma_i++]);
00148   for (int i = 0; i < state_n; i++) {
00149     CvMat col;
00150     cvGetCol(sqrtStateCovariance, &col, i);
00151     cvAdd(state, &col, sigma_state[sigma_i++]);
00152     cvSub(state, &col, sigma_state[sigma_i++]);
00153   }
00154 
00155   sigmasUpdated = true;
00156 }
00157 
00158 void UnscentedKalman::predict(UnscentedProcess *process_model) {
00159   if (!sigmasUpdated) initialize();
00160 
00161   // Map sigma points through the process model and compute new state mean.
00162   cvSetZero(state);
00163   double L = state_n + state_k;
00164   double totalWeight = 0;
00165   for (int i = 0; i < sigma_n; i++) {
00166     double weight = i == 0 
00167       ? lambda / (L + lambda)
00168       : .5 / (L + lambda);
00169         totalWeight += weight;
00170   }
00171   for (int i = 0; i < sigma_n; i++) {
00172     CvMat *sigma = sigma_state[i];
00173     process_model->f(sigma);
00174     double weight = i == 0 
00175       ? lambda / (L + lambda)
00176       : .5 / (L + lambda);
00177         double scale = weight / totalWeight;
00178     cvAddWeighted(sigma, scale, state, 1., 0., state);
00179   }
00180 
00181   // Compute new state co-variance.
00182   cvSetZero(stateCovariance);
00183   totalWeight = 0;
00184   for (int i = 0; i < sigma_n; i++) {
00185     double weight = i == 0 
00186       ? lambda / (L + lambda) + lambda2
00187       : .5 / (L + lambda);
00188         totalWeight += weight;
00189   }
00190   for (int i = 0; i < sigma_n; i++) {
00191     double weight = i == 0 
00192       ? lambda / (L + lambda) + lambda2
00193       : .5 / (L + lambda);
00194         double scale = weight / totalWeight;
00195     cvSub(sigma_state[i], state, stateDiff);
00196     cvGEMM(stateDiff, stateDiff, scale, stateCovariance, 1., stateCovariance,
00197            CV_GEMM_B_T);
00198   }
00199 
00200   // Add any additive noise.
00201   CvMat *noise = process_model->getProcessNoise();
00202   if (noise) cvAdd(stateCovariance, noise, stateCovariance);
00203 
00204 #ifdef MYDEBUG
00205   printf("predicted state: ");
00206   for (int i = 0; i < state_n; i++) printf("%f ", cvGetReal1D(state, i));
00207   printf("\n");
00208   printf("predicted stateCovariance:\n");
00209   for (int i = 0; i < state_n; i++) {
00210     for (int j = 0; j < state_n; j++) printf("%+f ", cvGetReal2D(stateCovariance, i, j));
00211     printf("\n");
00212   }
00213 #endif
00214 
00215   sigmasUpdated = false;
00216 }
00217 
00218 void UnscentedKalman::update(UnscentedObservation *obs) {
00219   if (!sigmasUpdated) initialize();
00220   CvMat *innovation = obs->getObservation();
00221   int obs_n = innovation->rows;
00222   if (obs_n > this->obs_n) {
00223     printf("Observation exceeds maximum size!\n");
00224     abort();
00225   }
00226 
00227   // Map sigma points through the observation model and compute predicted mean.
00228   CvMat predObs = cvMat(obs_n, 1, CV_64F, this->predObs->data.db);
00229   cvSetZero(&predObs);
00230   for (int i = 0; i < sigma_n; i++) {
00231     CvMat sigma_h = cvMat(obs_n, 1, CV_64F, sigma_predObs[i]->data.db);
00232     double scale = i == 0 
00233       ? (double)state_k / (double)(state_n + state_k)
00234       : .5 / (double)(state_n + state_k);
00235     obs->h(&sigma_h, sigma_state[i]);
00236     cvAddWeighted(&sigma_h, scale, &predObs, 1., 0., &predObs);
00237   }
00238 
00239   // Compute predicted observation co-variance.
00240   CvMat predObsCovariance = cvMat(obs_n, obs_n, CV_64F, 
00241                                   this->predObsCovariance->data.db);
00242   CvMat statePredObsCrossCorrelation = cvMat(state_n, obs_n, CV_64F, 
00243                                              this->statePredObsCrossCorrelation->data.db);
00244   CvMat predObsDiff = cvMat(obs_n, 1, CV_64F, this->predObsDiff->data.db);
00245   cvSetZero(&predObsCovariance);
00246   cvSetZero(&statePredObsCrossCorrelation);
00247   for (int i = 0; i < sigma_n; i++) {
00248     CvMat sigma_h = cvMat(obs_n, 1, CV_64F, sigma_predObs[i]->data.db);
00249     double scale = i == 0 
00250       ? (double)state_k / (double)(state_n + state_k)
00251       : .5 / (double)(state_n + state_k);
00252     cvSub(sigma_state[i], state, stateDiff);
00253     cvSub(&sigma_h, &predObs, &predObsDiff);
00254     cvGEMM(&predObsDiff, &predObsDiff, scale, &predObsCovariance, 1., &predObsCovariance,
00255                  CV_GEMM_B_T);
00256     cvGEMM(stateDiff, &predObsDiff, scale, &statePredObsCrossCorrelation, 1., 
00257                  &statePredObsCrossCorrelation, CV_GEMM_B_T);
00258   }
00259 
00260   // Add any additive noise.
00261   CvMat *noise = obs->getObservationNoise();
00262   if (noise) cvAdd(&predObsCovariance, noise, &predObsCovariance);
00263 
00264 #ifdef MYDEBUG
00265   printf("real observation: ");
00266   for (int i = 0; i < obs_n; i++) printf("%+f ", cvGetReal1D(innovation ,i));
00267   printf("\n");
00268   printf("predicted observation: ");
00269   for (int i = 0; i < obs_n; i++) printf("%+f ", cvGetReal1D(&predObs,i));
00270   printf("\n");
00271   printf("predicted observation co-variance\n");
00272   for (int i = 0; i < obs_n; i++) {
00273     for (int j = 0; j < obs_n; j++) printf("%+f ", cvGetReal2D(&predObsCovariance,i,j));
00274     printf("\n");
00275   }
00276   printf("state observation cross-correlation\n");
00277   for (int i = 0; i < state_n; i++) {
00278     for (int j = 0; j < obs_n; j++) printf("%+f ", cvGetReal2D(&statePredObsCrossCorrelation,i,j));
00279     printf("\n");
00280   }
00281 #endif
00282 
00283   // Update state mean and co-variance.
00284   //  innovation: v = z - pz
00285   //  gain: W = XZ * (R + Z)^-1
00286   //  state: x = x + _W * v
00287   //  co-var: P = P - W * (R + Z) * W^T
00288 
00289   CvMat invPredObsCovariance = cvMat(obs_n, obs_n, CV_64F, 
00290                                      this->invPredObsCovariance->data.db);
00291   CvMat kalmanGain = cvMat(state_n, obs_n, CV_64F, this->kalmanGain->data.db);
00292   CvMat kalmanTmp = cvMat(state_n, obs_n, CV_64F, this->kalmanTmp->data.db);
00293 
00294   cvSub(innovation, &predObs, innovation);
00295   //double inno_norm = cvNorm(innovation) / obs_n;
00296   //if (inno_norm > 5.0) {
00297   //  return;
00298   //}
00299 
00300 #ifdef MYDEBUG
00301   printf("innovation: ");
00302   for (int i = 0; i < obs_n; i++) printf("%f ", cvGetReal1D(innovation,i));
00303   printf("\n");
00304   double inn_norm = cvNorm(innovation);
00305   printf("innivation norm: %f\n", inn_norm);
00306 #endif
00307 
00308   cvInvert(&predObsCovariance, &invPredObsCovariance, CV_SVD_SYM);
00309   cvMatMul(&statePredObsCrossCorrelation, &invPredObsCovariance, &kalmanGain);
00310   cvGEMM(&kalmanGain, innovation, 1., state, 1., state);
00311   cvMatMul(&kalmanGain, &predObsCovariance, &kalmanTmp);
00312   cvGEMM(&kalmanTmp, &kalmanGain, -1., stateCovariance, 1., stateCovariance,
00313                CV_GEMM_B_T);
00314 #ifdef MYDEBUG
00315   printf("estimated state: ");
00316   for (int i = 0; i < state_n; i++) printf("%f ", cvGetReal1D(state, i));
00317   printf("\n");
00318 #endif
00319 
00320   sigmasUpdated = false;
00321 }
00322 
00323 } // namespace alvar


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Feb 16 2017 03:23:02