#ifndef KALMAN_FILTER_CPP
#define KALMAN_FILTER_CPP

#include "KalmanFilter.h"

CKalmanFilter::CKalmanFilter()
{
	m_pCar = NULL;
	if(m_pCar == NULL){
		m_pCar = new Car;
	}

	//m_pCar->InitMatrix();

	m_bFlag = false;
	m_nCounts = 0;
}

CKalmanFilter::~CKalmanFilter()
{
	if(m_pCar){
		delete m_pCar;
		m_pCar = NULL;
	}
}

/*
 * 函数名：Initial
 *		卡尔曼初始化条件
 *
 * param:
 *		car
 *		t
 *
 * return:
 *		无
 *
*/
void CKalmanFilter::Initial(double t)
{
	//t = 0.5;
	m_pCar->A.setIdentity(READER_NUMS, READER_NUMS);
	m_pCar->A(0, 1) = t;
	m_pCar->A(2, 3) = t;
	m_pCar->x.setZero(READER_NUMS, 1);
	m_pCar->P.setZero(READER_NUMS, READER_NUMS);
	m_pCar->P(0, 0) = 8000;
	m_pCar->P(1, 1) = 10;
	m_pCar->P(2, 2) = 8000;
	m_pCar->P(3, 3) = 10;
	m_pCar->Q.setZero(READER_NUMS, READER_NUMS);
	m_pCar->Q(0, 0) = 2.5;
	m_pCar->Q(1, 1) = 0.1;
	m_pCar->Q(2, 2) = 2.5;
	m_pCar->Q(3, 3) = 0.1;

	m_pCar->H.setIdentity(OBSERVATION_NUMS, OBSERVATION_NUMS);
	m_pCar->z.setZero(OBSERVATION_NUMS, 1);
	m_pCar->R.setIdentity(OBSERVATION_NUMS, OBSERVATION_NUMS);
	//以下4个参数用于惯导
	/*m_pCar->Q(1,0) = 100;
	m_pCar->Q(3,2) = 100;
	m_pCar->R(1,1) = 0.1;
	m_pCar->R(3,3) = 0.1;*/
	m_pCar->t = 0;
}

/*
 * 没有定位成功时调用
*/
void CKalmanFilter::Predict(double t)
{
	//t = 0.5;
	//predict
	m_pCar->A(0,1) = t;
	m_pCar->A(2,3) = t;
	m_pCar->x = m_pCar->A * m_pCar->x;
	m_pCar->P = m_pCar->A * m_pCar->P *(m_pCar->A).adjoint() + m_pCar->Q;
}

/*
 * 定位成功调用
*/
void CKalmanFilter::Predict_Correct(double t)
{
	//t = 0.5;
	//predict
	Predict(t);
	//correct
	Eigen::MatrixXd K = m_pCar->P * m_pCar->H.adjoint() * (m_pCar->H * m_pCar->P *m_pCar->H.adjoint() + m_pCar->R).inverse();
	m_pCar->x = m_pCar->x + K * (m_pCar->z - m_pCar->H * m_pCar->x);
	//m_pCar->P = m_pCar->P - K * m_pCar->H * m_pCar->P.adjoint();
	m_pCar->P = m_pCar->P - K * m_pCar->H * m_pCar->P;
}

#endif