kalman_filter.h

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

#ifndef _KALMAN_FILTER_H
#define _KALMAN_FILTER_H

#include "datasource.h"
#include "matrix.h"
#include <iostream>

class KalmanFilter
{
 public:
   //The number of dimensions in signal (ns)
   unsigned int num_signal_dimensions;
   
   //The number of dimensions in measurement (nm)
   unsigned int num_measurement_dimensions;
   
   //The relation between one signal and the next (ns*ns)
   matrix A;

   //The relation between measurement and signal (nm*ns)
   matrix H;

   //the state vector-actually a column matrix (ns*1)
   matrix x;

   //The prediction vector-actually a column matrix (ns*1)
   //matrix x_pred;

   //The measurement vector-again a column matrix (nm*1)
   matrix z;

   //The error covariance matrix (ns*ns)
   matrix P;

   //The Kalman gain matrix (ns*nm)
   //matrix K;
   //The covariance matrix of system (ns*ns)
   matrix Q;
   //The covariance matrix of measurement (nm*nm)
   matrix R;

  public:
 KalmanFilter(unsigned int ns,unsigned int nm,
                   matrix& Ai,
                   matrix& Hi,
                   matrix& Qi,
				   matrix& Ri,
                   matrix& x_initial,
                   matrix& Pi);
   matrix& next(matrix& z);
   inline matrix estimate(){return x;}
  // return the current signal prediction
   //inline matrix prediction(){return x_pred;}
   //friend std::ostream& operator<<(std::ostream &os,  KalmanFilter &kf);
 };
#endif