kalman_filter.cpp

这是卡尔曼滤波算法用在数据融合中的程序实现

///////////////////////////////////////////////////////////////////////////////
//This is Kalman Filter .cpp
//:
// \file
// \brief kalman_filter.cpp: A linear Kalman filter class
//
// Implementation of a linear Kalman filter. We want to estimate the
// value from a measurement and predict the next value of the signal.
//
// The relationship between signal and measurement is:
// z(k)        =  H(k)    *    x(k)     +    v(k)
// measurement    relation     signal        noise
//                matrix
//
// The relationship between signals at different times is:
// x(k+1)      =  A(k)    *    x(k)     +   w(k)
// signal         relation     signal       noise
//                matrix                        
//
// 
///////////////////////////////////////////////////////////////////////////////

#include "kalman_filter.h"
#include <iostream>
#include <cstdlib>
#include <cmath>
#include <string>


#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

///////////////////////////////////////////////////////////////////////////////
// Constructor
//
// Initialises the necessary parameters and matrices
//
// \param ns  number of dimensions in signal
// \param nm  number of dimensions in measurement
//
// \param A   the ns*ns matrix relating signal at time k to signal at time k+1.
//
// \param H   the nm*ns matrix relating the measurement to the signal.
//
// \param x_initial  the initial ns*1 estimate of the signal.
//
// \param p_initial  the initial error variance vector.
//        P = p_initial.transpose()*p_initial.
//
// \status under development
////////////////////////////////////////////////////////////////////////////////
KalmanFilter::KalmanFilter(unsigned int ns,unsigned int nm,
                   matrix& Ai,
                   matrix& Hi,
                   matrix& Qi,
				   matrix& Ri,
                   matrix& x_initial,
                   matrix& Pi):
   A(Ai),H(Hi), Q(Qi),R(Ri),x(x_initial),P(Pi), z(Hi.rownum,1)
{
	z=0.0;
	A = Ai;
	Q = Qi;
	H = Hi;
	R = Ri;
}

matrix& KalmanFilter::next(matrix& z)
{
	x = A*x;
	P = A*P*A.t() + Q;
	matrix K = P*H.t()*~(H*P*H.t() + R);
	x += K*(z-H*x);
	P = (unit(P.rownum)-K*H)*P;
	return x;
}
/*
ostream &operator<<(ostream &os,  KalmanFilter &kf)
{
   os << "Current state of Kalman Filter is:\n"
      << "estimate = " << kf.x.t()
      << "prediction = " << kf.x_pred.t()
      << "measurement = " << kf.z.t()
      << "error covariance = \n" << kf.P
      << "A =\n" << kf.A
      << "H =\n" << kf.H;
   return os;
}
*/