#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