//#pragma once
#ifndef KALMAN_FILTER_H
#define KALMAN_FILTER_H

#include <Eigen/Dense>

//用于卡尔曼滤波
const int READER_NUMS = 4;
const int OBSERVATION_NUMS = 4;

class Car;

class CKalmanFilter{
public:
	Car* m_pCar;
public:
	bool m_bFlag;	//定位标志位
	int  m_nCounts; //定位成功数
public:
	CKalmanFilter();
	~CKalmanFilter();
public:
	void Initial(double t);
	void Predict(double t);
	void Predict_Correct(double t);
};

class Car
{	
public:	
	double t;
	Eigen::Matrix<double, READER_NUMS, READER_NUMS,Eigen::DontAlign> A;
	//滤波结果保存，0 - 3 分别代表x位置坐标 x 方向速度 y坐标 y速度
	Eigen::Matrix<double, READER_NUMS, 1,Eigen::DontAlign> x;
	Eigen::Matrix<double, READER_NUMS, READER_NUMS,Eigen::DontAlign> P;
	Eigen::Matrix<double, READER_NUMS, READER_NUMS,Eigen::DontAlign> Q;
	Eigen::Matrix<double, OBSERVATION_NUMS, OBSERVATION_NUMS,Eigen::DontAlign> H;
	Eigen::Matrix<double, OBSERVATION_NUMS, OBSERVATION_NUMS,Eigen::DontAlign> R;
	//原始定位结果，0 - 3 分别代表x位置坐标 x 方向速度 y坐标 y速度
	Eigen::Matrix<double, OBSERVATION_NUMS, 1,Eigen::DontAlign> z;
public:
	//EIGEN_MAKE_ALIGNED_OPERATOR_NEW			//数据结构对齐
};

#endif