UnscentedKalman.h

Go to the documentation of this file.

/*
 * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
 *
 * Copyright 2007-2012 VTT Technical Research Centre of Finland
 *
 * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
 *          <http://www.vtt.fi/multimedia/alvar.html>
 *
 * ALVAR is free software; you can redistribute it and/or modify it under the
 * terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation; either version 2.1 of the License, or (at your option)
 * any later version.
 *
 * This library is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
 * for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with ALVAR; if not, see
 * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
 */

#ifndef __UNSCENTED_KALMAN__
#define __UNSCENTED_KALMAN__

#include "Alvar.h"
#include "cxcore.h"

namespace alvar {

  class UnscentedProcess;
  class UnscentedObservation;

  class ALVAR_EXPORT UnscentedKalman {
  private:
    int state_n;
    int state_k;
    int obs_n;
    int sigma_n;
    bool sigmasUpdated;
        double lambda, lambda2;

    CvMat *state;
    CvMat *stateCovariance;
    CvMat *sqrtStateCovariance;
    CvMat *stateD;
    CvMat *stateU;
    CvMat *stateV;
    CvMat *stateTmp;
    CvMat *stateDiff;

    CvMat *predObs;
    CvMat *predObsCovariance;
    CvMat *invPredObsCovariance;
    CvMat *predObsDiff;

    CvMat *statePredObsCrossCorrelation;
    CvMat *kalmanGain;
    CvMat *kalmanTmp;

    CvMat **sigma_state;
    CvMat **sigma_predObs;

    // possess state mean and co-variance (as a list of sigma points).
    // generate sigma points from state mean vector and co-variance matrix.
    // compute state mean vector and co-variance matrix from sigma points.

    // predict: 
    //  - map sigma points thru process model f.

    // update:
    //  - map sigma points thru h.
    //  - from current sigma points and sigma observations:
    //   - compute state estimate x and co-variance P.
    //   - compute predicted observation z and innocation co-variance Z
    //   - compute cross correlation XZ
    //  - compute new state mean and co-variance.
    //  - generate new sigma points.
  public:

    UnscentedKalman(int state_n, int obs_n, int state_k = 0, double alpha = 0.001, double beta = 2.0);
    ~UnscentedKalman();

    CvMat *getState() { return state; }

    CvMat *getStateCovariance() { return stateCovariance; }

    void initialize();

    void predict(UnscentedProcess *process_model);

    void update(UnscentedObservation *observation);
  };

  class ALVAR_EXPORT UnscentedProcess {
  public:
    virtual void f(CvMat *state) = 0;

    virtual CvMat *getProcessNoise() = 0;
  };

  class ALVAR_EXPORT UnscentedObservation {
  public:
    virtual void h(CvMat *z, CvMat *state) = 0;

    virtual CvMat *getObservation() = 0;

    virtual CvMat *getObservationNoise() = 0;
  };

} // namespace alvar

#endif // __UNSCENTED_KALMAN__