/home/jgoppert/Projects/uvsim/uvsim/src/uvsim/navigation/KalmanFilter.cc

Go to the documentation of this file.

/*
 * KalmanFilter.cpp
 *
 *  Created on: Apr 28, 2009
 *      Author: jgoppert
 */

#include "KalmanFilter.h"

#include <boost/numeric/ublas/matrix_expression.hpp>
#include <uvsim/utilities/utilities.h>
#include <iostream>

using namespace boost::numeric::ublas;

namespace uvsim
{

KalmanFilter::KalmanFilter(LinearModel * model, vector<double> *y, matrix<double> *P) :
        model(model), y(y), P(P)
{
}

KalmanFilter::~KalmanFilter()
{
    // TODO Auto-generated destructor stub
}

void KalmanFilter::update()
{
    *P = prod(model->Ad, *P, trans(model->Ad)) + prod(model->Ed,trans(model->Ed));
        *(model->x) = prod(model->Ad,*(model->x)) + prod(model->Bd,*(model->u));

    matrix<double> S;
    S = prod(model->C, *P, trans(model->C)) + prod(model->L,trans(model->L));
    K = prod(*P, trans(model->C), inv(S));
        *P = *P - prod(K, model->C, *P);
    *(model->x) = *(model->x) + prod(K, *y - prod(model->C,*(model->x)));

}

void KalmanFilter::print()
{
    std::cout << "x: " << *(model->x) << std::endl;
        std::cout << "P: " << *P << std::endl;
        std::cout << "K: " << K << std::endl;

}

}

// vim:ts=4:sw=4

---