mystruct.h

VC.NET KalmanFilter.rar

#include "Matrix.h"


/*
standard Kalman filter (in G. Welch' and G. Bishop's notation):

  x(k)=A*x(k-1)+B*u(k)+w(k)  p(w)~N(0,Q)
  z(k)=H*x(k)+v(k),   p(v)~N(0,R)
*/
typedef struct Kalman
{
    int MP;                     /* number of measurement vector dimensions */
    int DP;                     /* number of state vector dimensions */
    int CP;                     /* number of control vector dimensions */

    Matrix state_pre;           /* predicted state (x'(k)):
                                    x(k)=A*x(k-1)+B*u(k) */
    Matrix state_post;          /* corrected state (x(k)):
                                    x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) */
    Matrix transition_matrix;   /* state transition matrix (A) */
    Matrix control_matrix;      /* control matrix (B)
                                   (it is not used if there is no control)*/
    Matrix measurement_matrix;  /* measurement matrix (H) */
    Matrix process_noise_cov;   /* process noise covariance matrix (Q) */
    Matrix measurement_noise_cov; /* measurement noise covariance matrix (R) */
    Matrix error_cov_pre;       /* priori error estimate covariance matrix (P'(k)):
                                    P'(k)=A*P(k-1)*At + Q)*/
    Matrix gain;                /* Kalman gain matrix (K(k)):
                                    K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)*/
    Matrix error_cov_post;      /* posteriori error estimate covariance matrix (P(k)):
                                    P(k)=(I-K(k)*H)*P'(k) */
    Matrix temp1;               /* temporary matrices */
    Matrix temp2;
    Matrix temp3;
    Matrix temp4;
    Matrix temp5;
}
Kalman;

class CKalmanFilter
{
	public:
		Kalman kalman;
		CKalmanFilter(int MP,int DP,int CP);
// 		~CKalmanFilter();
		Matrix KalmanPredict(Kalman &kalman,const Matrix control);
		Matrix KalmanPredict(Kalman &kalman);
		Matrix KalmanUpdate(Kalman  &kalman, const Matrix measurement);
		BOOL InitKalman(Kalman &kalman,Matrix stateInit,Matrix covInit, Matrix transitionMatrix, 
							   Matrix measureMatrxi,Matrix processNoiseCov, Matrix measureNoiseCov,
							   Matrix controlMatrix);
		BOOL InitKalman(Kalman &kalman,Matrix stateInit,Matrix covInit, Matrix transitionMatrix,
			Matrix measureMatrxi,Matrix processNoiseCov, Matrix measureNoiseCov);
		Matrix GetInitState(Matrix measurement,double timeStep);
		Matrix GetInitCov(Matrix measurement,double timeStep);
	protected:
	private:
		
};