/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name          : Kalman.c
* Current Version    : V2.0
* Author             : logzhan
* Date of Issued     : 2022.10.15
* Comments           : PDR扩展卡尔曼滤波器融合
********************************************************************************/
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <malloc.h>
#include "PDRBase.h"
#include "Kalman.h"
#include "Location.h"
#include "Matrix.h"
#include "Utils.h"

// 扩展卡尔曼向量缓存
static double EkfVecBuf[5][N] = { {0.0} };
// 扩展卡尔曼矩阵缓存
static double EkfMatBuf[7][N][N] = { { {0.0} } };

/**----------------------------------------------------------------------
* Function    : KalmanFilter_Init
* Description : 初始化卡尔曼滤波器相关
* Date        : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
void EKF_Init(void)
{
	memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
	memset(EkfVecBuf, 0, sizeof(double) * 5 * N);
}

/**----------------------------------------------------------------------
* Function    : EKFStatePredict
* Description : pdr卡尔曼滤波器的状态预测方程
* Date        : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void EKFStatePredict(EKFPara_t *ekf, double stepLen, PDR_t* pdr, int step) {

	double angle = pdr->deltaHeading + ekf->pXk[3];

	if(step == 0){
		ekf->pXk[3] = angle;
		return;
	}

	double(*Phi)[N] = (double(*)[N]) & (EkfMatBuf[0][0][0]);
	double(*pvec1)[N] = (double(*)[N]) & (EkfMatBuf[1][0][0]);
	double(*pvec2)[N] = (double(*)[N]) & (EkfMatBuf[2][0][0]);

	memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);

    // xk+1 = xk + step * cos(angle)   PDR位置更新方程
	// yk+1 = yk + step * sin(angle)   PDR位置更新方程
	// sk+1 = sk                       步长和上一次相同
	// ak+1 = ak * (angle / ak) 或 ak+1 = ak

	Phi[0][0] = 1;            Phi[1][0] = 0;          Phi[2][0] = 0;  Phi[3][0] = 0;
	Phi[0][1] = 0;			  Phi[1][1] = 1;		  Phi[2][1] = 0;  Phi[3][1] = 0;
	Phi[0][2] = cos(angle);	  Phi[1][2] = sin(angle); Phi[2][2] = 1;  Phi[3][2] = 0;
	Phi[0][3] = 0;            Phi[1][3] = 0;          Phi[2][3] = 0;  Phi[3][3] = 1;


	for (uchar i = 0; i < step; i++) {
		// logzhan 21/02/06:
		// phi[3][3]是为让kf的预测每次都是最新更新值, 如果phi[3][3]每次都是1，可能在惯性
		// 导航更新过程中，偏航角的预测是固定的，实时性没有那么好，下面if判断可以在一定程度
		// 让预测的值更新，但是效果暂时没有感觉出明显的好
		if (fabs(ekf->pXk[3]) > 0.000001 &&
			fabs(angle / ekf->pXk[3]) < 3)
		{
			Phi[3][3] = angle / ekf->pXk[3];
		}

		// 卡尔曼状态更新方程
		// pXk = phi * pXk
		VecMatMultiply(ekf->pXk, Phi, ekf->pXk);

		// 卡尔曼更新协方差矩阵 : Pk = FPFt + Q
		MatrixMultiply(Phi, ekf->pPk, pvec1);               // F*P
		MatrixTrans(Phi, pvec2);                            // Ft
		MatrixMultiply(pvec1, pvec2, pvec1);                // F*P*Ft
		MatrixAdd(pvec1, ekf->Q, ekf->pPk);                 // F*P*Ft + Q
	}
}

/**----------------------------------------------------------------------
* Function    : EKFStateUpdate
* Description : 扩展卡尔曼滤波器的状态更新方程
* Date        : 2020/7/22 logzhan
*---------------------------------------------------------------------**/
void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss,  Classifer_t *sys, PDR_t *pdr, 
	int scene_type) {

	double lambda = 0;

	// NxN矩阵定义
	double(*H)[N]     = (double(*)[N])&(EkfMatBuf[0][0][0]);
	double(*I)[N]     = (double(*)[N])&(EkfMatBuf[1][0][0]);
	double(*pvec1)[N] = (double(*)[N])&(EkfMatBuf[2][0][0]);
	double(*Ht)[N]    = (double(*)[N])&(EkfMatBuf[3][0][0]);
	double(*pvec3)[N] = (double(*)[N])&(EkfMatBuf[4][0][0]);
	double(*pvec4)[N] = (double(*)[N])&(EkfMatBuf[5][0][0]);
	double(*pv3)[N]   = (double(*)[N])&(EkfMatBuf[6][0][0]);

	memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
	memset(EkfVecBuf, 0, sizeof(double) * 5 * N);

	// Nx1向量定义
	double(*pvec5)    = &(EkfVecBuf[1][0]);
	double(*pvec6)    = &(EkfVecBuf[2][0]);
	double(*pv1)      = &(EkfVecBuf[3][0]);
	double(*pv2)      = &(EkfVecBuf[4][0]);
	
	// 创建单位矩阵
	for (char i = 0; i < 4; i++) {
		H[i][i] = 1;
		I[i][i] = 1;
	}

	double* z = &(EkfVecBuf[0][0]);
	// 获取观测向量
	z[0] = pgnss->xNed;                                          // 北向位置
	z[1] = pgnss->yNed;                                          // 东向位置   
	z[2] = 0.5;                                                  // 步长默认0.5

	// 这个if条件实际是使用GPS的位置变化量和步数变化量估计步长, 这部分代码还没
	// 调好，实际类似于步长不变
	//if (pgnss->error > 0 && pgnss->error < 3.8 && pgnss->HDOP <= 0.4 && 
	//	g_pdr->deltaStepsPerGps != 0 && g_pdr->deltaStepsPerGps < 5) {
	//	// 利用GPS速度和步频估计步长
	//	//if (g_pdr->motionFreq > 2.5 && (sys->type == 1)) {
	//	//	
	//	//	//printf("z[2] = %f\n", z[2]);
	//	//}
	//	z[2] = pgnss->vel * 0.514 / (g_pdr->deltaStepsPerGps);
	//}else {
	//	z[2] = kf->pXk[2];
	//}

	z[3] = pgnss->yaw * PI / 180;                                // 观测的航向角为GPS的航向角

	// 计算 K = K = PHt / (R + H*P*H^T)
	MatrixTrans(H, Ht);                                             // 计算Ht
	MatrixMultiply(H, kf->pPk, pvec1);                             // 计算HP
	MatrixMultiply(pvec1, Ht, pvec3);                               // pvec3 = H*P*H^T
	MatrixAdd(pvec3, kf->R, pvec4);                                 // pvec4 = R + H*P*H^T
	MatrixInverse(pvec4, pvec1);                                    // 计算 1 / (R + H*P*H^T)

	for (char i = 0; i < N; i++) {
		for (uchar j = 0; j < N; j++) {
			pv3[i][j] = pvec3[i][j];
		}
	}
	MatrixMultiply(kf->pPk, Ht, pvec3);                                  // 计算PHt
	MatrixMultiply(pvec3, pvec1, kf->Kk);                                 // 计算增益K = PHt / (R + H*P*H^T)
								                                  
								                                  
	VecMatMultiply(kf->pXk, H, pvec5);                                   // pvec5 = Hx'
	VectorSub(z, pvec5, pvec6);                                       // pvec6 = Z - Hx'
	modAngle(&pvec6[3], -PI, PI);
	for (char i = 0; i < N; i++) {
		pv1[i] = pvec6[i];
	}
	
	VecMatMultiply(pvec6, kf->Kk, pvec5); // pvec5 = K*( z - Hx')
	// Get the optimally updated state estimation
	VectorAdd(kf->pXk, pvec5, kf->Xk);

	// ---- prepare the matrix for updated estimate covariance ------
	// pvec1 = K*H
	MatrixMultiply(kf->Kk, H, pvec1);
	// pvec2 = (I - K*H)
	MatrixSub(I, pvec1, Ht);
	// pvec3 = (I - K*H)*P
	MatrixMultiply(Ht, kf->pPk, pvec3);
	// pvec4 = (I- K*H)^T
	MatrixTrans(Ht, pvec4);
	// pvec1 = (I - K*H)*P*(I- K*H)^T
	MatrixMultiply(pvec3, pvec4, pvec1);

	// pvec2 = K*R
	MatrixMultiply(kf->Kk, kf->R, Ht);
	// pvec3 = K^T
	MatrixTrans(kf->Kk, pvec3);
	// pvec4 = K*R*K^T
	MatrixMultiply(Ht, pvec3, pvec4);
	// Get the updated estimate covariance: P' = (I - K*H)*P*(I- K*H)^T + K*R*K^T
	MatrixAdd(pvec1, pvec4, kf->Pk);

	// pv2 = (z - Hx')*( H*P*H^T )
	VecMatMultiply(pv1, pv3, pv2);

	// inner product the pre-fit residual and the transitioned residual: lamda = (z - Hx')*( H*P*H^T )  .*  (z-Hx')
	// Calculate the difference between original differenc and the transitioned,
	// if the value is close to zero, it shows that there is a big gap in the 
	// oprimization. Thus, we believe our PDR algorithm
	for (char i = 0; i < N; i++) {
		lambda += pv2[i] * pv1[i];
	}

	kf->Lambda = lambda;
	kf->pLat = pgnss->lat;
	kf->pLon = pgnss->lon;

	if (lambda >= 200)return;

	for (char i = 0; i < N; i++) {
		kf->Xk[i] = kf->pXk[i];
		for (int j = 0; j < N; j++) {
			kf->Pk[i][j] = kf->pPk[i][j];
		}
	}
	
}

/**----------------------------------------------------------------------
* Function    : EKFCalQRMatrix
* Description : 根据GPS信号特征调整卡尔曼滤波噪声矩阵，q矩阵是过程噪声矩阵，
*               需要跟惯导预测位置精度相关。r矩阵为GNSS观测噪声，跟GPS输出的
*               信息精度有关。
* Date        : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
void EKFCalQRMatrix(Nmea_t *nmea, PDR_t *pdr, Classifer_t *sys,
	EKFPara_t *kf) {

	if (pdr->MotionType != PDR_MOTION_TYPE_HANDHELD && 
		pdr->MotionType != PDR_MOTION_TYPE_STATIC) {
		kf->Q[0][0] = 25;                   // PDR预测位置噪声
		kf->Q[1][1] = 25;                   // PDR预测位置噪声
		kf->Q[2][2] = 1;                    // PDR步长噪声
		kf->Q[3][3] = 100;                  // PDR方向噪声

		kf->R[0][0] = 50;                    // GPS观测位置噪声
		kf->R[1][1] = 50;					 // GPS观测位置噪声
		kf->R[2][2] = 0.1;					 // GPS步长噪声
		kf->R[3][3] = 50;					 // GPS方向噪声
		return;
	}

	kf->Q[0][0] = 0.1;                       // PDR预测位置噪声
	kf->Q[1][1] = 0.1;                       // PDR预测位置噪声
	kf->Q[2][2] = 1;                         // PDR步长噪声
	kf->Q[3][3] = 1;                         // PDR方向噪声
										     
	kf->R[0][0] = 100;                       // GPS预测位置噪声
	kf->R[1][1] = 100;					     // GPS预测位置噪声
	kf->R[2][2] = 0.1;					     // GPS步长噪声
	kf->R[3][3] = 1000;				         // GPS方向噪声

	if (nmea->accuracy.err > 0 && 
		nmea->accuracy.err < 3.80 && 
		nmea->gga.hdop <= 0.4) 
	{
		kf->R[0][0] = 50;                    // GPS观测位置噪声
		kf->R[1][1] = 50;					 // GPS观测位置噪声
		kf->R[2][2] = 0.1;					 // GPS步长噪声
		kf->R[3][3] = 50;					 // GPS方向噪声
	}

	if (fabs(R2D((pdr->GpsHeading - pdr->LastGpsHeading))) > 50 ||
		pdr->GpsSpeed < 0.7 || pdr->GpsHeading == -1) {
		kf->R[3][3] = 10000;
	}
}

