/******************** (C) COPYRIGHT 2020 VIVO************************************
* File Name          : pdr_kalman.c
* Department         : Sensor Algorithm Team
* Current Version    : V2.0(compare QCOM SAP 5.0)
* Author             : yuanlin_rjyb@vivo.com & zhanli@vivo.com
* Date of Issued     : 2020.7.3
* Comments           : PDR 卡尔曼滤波器相关
********************************************************************************/
#include <stdio.h>
#include <string.h>
#include <math.h>
#if !defined(LOCATION_PLATFORM_QCOM_MODEM)
#include <malloc.h>
#endif
#include "pdr_base.h"
#include "pdr_kalman.h"
#include "pdr_location.h"
#include "pdr_matrix.h"
#include "pdr_util.h"
#include "pdr_linearFit.h"

#if USE_BUG_FOR_LOCAL_PARA
static double Ekf_Upd_buf_2d[5][N] = { {0.0} };
static double Ekf_Upd_buf_3d[7][N][N] = { { {0.0} } };
#endif

extern double GpsPos[HIST_GPS_NUM][3];

extern int debugCount;
extern double angle_mean[3];

static int hold_type;
static double  b_timestamp;
static double  b_last_time;
static double  attitude_yaw;
static DETECTOR *g_detector;
static int hold_type;
static double BUL_duration;
static double FUR_duration;

/**----------------------------------------------------------------------
* Function    : pdr_initKalman
* Description : 初始化卡尔曼滤波器相关
* Date        : 2020/8/27 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void pdr_initKalman(void)
{
	b_timestamp  = -1;
	b_last_time  = -1;
	BUL_duration = -1;
	FUR_duration = -1;
	attitude_yaw = -1;
	hold_type = 0;
}

int clear_buf(double *buf_p, int dmt)
{
	int bufLen = 0;
	double *buf_pdr = (double *)buf_p;

	if (NULL == buf_p)return -1;

	if (!(2 == dmt || 1 == dmt)){
		return -1;
	}
	else if (2 == dmt)bufLen = N * N;
	else if (1 == dmt)bufLen = N;

	for (int i = 0; i < bufLen; i++){
		buf_pdr[i] = 0.0f;
	}
	return 0;
}

/**----------------------------------------------------------------------
* Function    : pdr_ekfStatePredict
* Description : pdr卡尔曼滤波器的状态预测方程
* Date        : 2020/7/22 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void pdr_ekfStatePredict(KfPara *kf, double stepLen, PDR* g_pdr, int step) {

	double (*phi)[N]   = (double(*)[N])&(Ekf_Upd_buf_3d[0][0][0]);
	double (*pvec1)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[1][0][0]);
	double (*pvec2)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[2][0][0]);

	clear_buf((double *)phi, BUF_DMT_1);
	clear_buf((double *)pvec1, BUF_DMT_1);
	clear_buf((double *)pvec2, BUF_DMT_1);

	double deltaHeading = g_pdr->heading - g_pdr->lastHeading;
	double angle        = g_pdr->heading;

	//printf("deltaHeading = %f\n", R2D(deltaHeading));

	//if (fabs(deltaHeading) < D2R(10) && g_pdr->lastHeading != 0.0f) {
	//	angle = deltaHeading + kf->p_xk[3];
	//}else {
	//	angle = g_pdr->heading;
	//}

	if (g_pdr->motionType != PDR_MOTION_TYPE_HANDHELD) {
		deltaHeading = 0;
	}

	angle = deltaHeading + kf->p_xk[3];


	if (fabs(deltaHeading) > 5.5) {
		//printf("%f %f\n", R2D(g_pdr->heading), R2D(g_pdr->lastHeading));
		//printf("d heading = %f\n", R2D(deltaHeading));
		if (deltaHeading < 0) {
			deltaHeading = deltaHeading + D2R(360);
		}else {
			deltaHeading = deltaHeading - D2R(360);
		}
		deltaHeading = 0;
		angle = deltaHeading + kf->p_xk[3];
	}

	if(step == 0){
		kf->p_xk[3] = angle;
	}



    // 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++) {
		// zhanli@vivo.com 21/02/06:
		// phi[3][3]是为让kf的预测每次都是最新更新值, 如果phi[3][3]每次都是1，可能在惯性
		// 导航更新过程中，偏航角的预测是固定的，实时性没有那么好，下面if判断可以在一定程度
		// 让预测的值更新，但是效果暂时没有感觉出明显的好
		if (fabs(kf->p_xk[3]) > 0.000001 &&
			fabs(angle / kf->p_xk[3]) < 3)
		{
			phi[3][3] = angle / kf->p_xk[3];
		}

		// 卡尔曼状态更新方程
		// p_xk = phi * p_xk
		vm_mlt(kf->p_xk, phi, kf->p_xk);

		// 卡尔曼更新协方差矩阵 : Pk = FPFt + Q
		mm_mlt(phi, kf->p_pk, pvec1);               // F*P
		m_trans(phi, pvec2);                        // Ft
		mm_mlt(pvec1, pvec2, pvec1);                // F*P*Ft
		m_add(pvec1, kf->q, kf->p_pk);             // F*P*Ft + Q
	}
}

/**----------------------------------------------------------------------
* Function    : pdr_ekfStateUpdate
* Description : pdr卡尔曼滤波器的状态更新方程
* Date        : 2020/7/22 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void pdr_ekfStateUpdate(KfPara *kf, gnss *pgnss,  classifer *sys, PDR *g_pdr, 
	int scene_type) {

	int j;

	double(*H)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[0][0][0]);
	double *z = &(Ekf_Upd_buf_2d[0][0]);
	double(*I)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[1][0][0]);
	double(*pvec1)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[2][0][0]);
	double(*Ht)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[3][0][0]);
	double(*pvec3)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[4][0][0]);
	double(*pvec4)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[5][0][0]);
	double(*pvec5) = &(Ekf_Upd_buf_2d[1][0]);
	double(*pvec6) = &(Ekf_Upd_buf_2d[2][0]);

	double lambda = 0;
	double(*pv1) = &(Ekf_Upd_buf_2d[3][0]);
	double(*pv2) = &(Ekf_Upd_buf_2d[4][0]);
	double(*pv3)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[6][0][0]);

	clear_buf((double *)z, BUF_DMT_1);
	clear_buf((double *)pvec5, BUF_DMT_1);
	clear_buf((double *)pvec6, BUF_DMT_1);
	clear_buf((double *)pv1, BUF_DMT_1);
	clear_buf((double *)pv2, BUF_DMT_1);

	clear_buf((double *)H, BUF_DMT_2);
	clear_buf((double *)I, BUF_DMT_2);
	clear_buf((double *)pvec1, BUF_DMT_2);
	clear_buf((double *)Ht, BUF_DMT_2);
	clear_buf((double *)pvec3, BUF_DMT_2);
	clear_buf((double *)pvec4, BUF_DMT_2);
	clear_buf((double *)pv3, BUF_DMT_2);


	for (char i = 0; i < 4; i++) {
		H[i][i] = 1;
		I[i][i] = 1;
	}

	// 获取观测向量
	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->p_xk[2];
	//}

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

	// 计算 K = K = PHt / (R + H*P*H^T)
	m_trans(H, Ht);                                              // 计算Ht
	mm_mlt(H, kf->p_pk, pvec1);                                  // 计算HP
	mm_mlt(pvec1, Ht, pvec3);                                    // pvec3 = H*P*H^T
	m_add(pvec3, kf->r, pvec4);                                 // pvec4 = R + H*P*H^T
	m_inverse(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];
		}
	}
	mm_mlt(kf->p_pk, Ht, pvec3);                                  // 计算PHt
	mm_mlt(pvec3, pvec1, kf->Kk);                                 // 计算增益K = PHt / (R + H*P*H^T)
								                                  
								                                  
	vm_mlt(kf->p_xk, H, pvec5);                                   // pvec5 = Hx'
	v_sub(z, pvec5, pvec6);                                       // pvec6 = Z - Hx'
	modAngle(&pvec6[3], -PI, PI);
	for (char i = 0; i < N; i++) {
		pv1[i] = pvec6[i];
	}
	
	vm_mlt(pvec6, kf->Kk, pvec5); // pvec5 = K*( z - Hx')
	// Get the optimally updated state estimation
	v_add(kf->p_xk, pvec5, kf->xk);

	// ---- prepare the matrix for updated estimate covariance ------
	// pvec1 = K*H
	mm_mlt(kf->Kk, H, pvec1);
	// pvec2 = (I - K*H)
	m_sub(I, pvec1, Ht);
	// pvec3 = (I - K*H)*P
	mm_mlt(Ht, kf->p_pk, pvec3);
	// pvec4 = (I- K*H)^T
	m_trans(Ht, pvec4);
	// pvec1 = (I - K*H)*P*(I- K*H)^T
	mm_mlt(pvec3, pvec4, pvec1);

	// pvec2 = K*R
	mm_mlt(kf->Kk, kf->r, Ht);
	// pvec3 = K^T
	m_trans(kf->Kk, pvec3);
	// pvec4 = K*R*K^T
	mm_mlt(Ht, pvec3, pvec4);
	// Get the updated estimate covariance: P' = (I - K*H)*P*(I- K*H)^T + K*R*K^T
	m_add(pvec1, pvec4, kf->pk);

	// pv2 = (z - Hx')*( H*P*H^T )
	vm_mlt(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->p_xk[i];
		for (j = 0; j < N; j++) {
			kf->pk[i][j] = kf->p_pk[i][j];
		}
	}
	
}

void calStepLastTime(StepPredict *stepPredict, double timestamp, unsigned long steps_last, 
	unsigned long steps)
{
	float tmpTime = 0;
	if (stepPredict->lastTime > 0 && stepPredict->deltaStep == 0 && (steps - steps_last) > 0)
	{
		tmpTime = (float)((timestamp - stepPredict->lastTime) / (steps - steps_last));
		if (tmpTime > 300 && tmpTime < 800)
		{
			if (stepPredict->meanTime > 0){
				stepPredict->meanTime = stepPredict->meanTime * 0.5f + tmpTime * 0.5f;
			}else{
				stepPredict->meanTime = tmpTime;
			}
		}
	}

	stepPredict->fnum = 0;
	stepPredict->fsum = 0;

	stepPredict->lastTime = timestamp;
	stepPredict->deltaStep = 0;

}

int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_last, float gnssVel)
{
	int step = 0;
	float mean_time = 400;
	double dis = 0;

	if (stepPredict->meanTime > 0)
		mean_time = stepPredict->meanTime;

	if ((steps_last > 0) && (stepPredict->fnum > 0) && (timestamp - stepPredict->lastTime > mean_time * 3) && (stepPredict->lastTime > 0))
	{
		stepPredict->fsum = stepPredict->fsum / stepPredict->fnum;		
		dis = sqrt(pow(GpsPos[HIST_GPS_NUM - 1][0]- GpsPos[HIST_GPS_NUM - 2][0], 2) + pow(GpsPos[HIST_GPS_NUM - 1][1] - GpsPos[HIST_GPS_NUM - 2][1], 2)
			 + pow(GpsPos[HIST_GPS_NUM - 1][2] - GpsPos[HIST_GPS_NUM - 2][2], 2));

		if (stepPredict->fsum != 0.0 && (gnssVel > 1.0 || (dis > 1.0 && dis < 3.0)))
		{
			step = (int)((timestamp - stepPredict->lastTime) / mean_time);
			stepPredict->deltaStep += step;
		}

		stepPredict->lastTime = timestamp;
		stepPredict->fnum = 0;
		stepPredict->fsum = 0;
	}

	return step;
}

/**----------------------------------------------------------------------
* Function    : stateRecognition
* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳  0: 平稳
* Date        : 2020/7/22 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void stateRecognition(float *acc, classifer *sys) {
	int i,j,k;
	float accNorm = 0;
	float sum = 0, max_min = 0, max = -1, min = 999;
	float mean = 0, std = 0;

	// 先计算当前加速度的模
	accNorm = (float)sqrt(vivopow(acc[0], 2) + vivopow(acc[1], 2) + vivopow(acc[2], 2));

	// 缓存更新
	for (i = 0; i < PATTERN_RECOGNITION_LEN - 1; i++) {
		sys->accBuffer[i] = sys->accBuffer[i + 1];
	}
	sys->accBuffer[PATTERN_RECOGNITION_LEN - 1] = accNorm;

	// 计算最大最小值和sum
	for (j = 0; j < PATTERN_RECOGNITION_LEN - 1; j++) {
		sum += sys->accBuffer[j];
		if (sys->accBuffer[j] > max) {
			max = sys->accBuffer[j];
		}
		if (sys->accBuffer[j] < min) {
			min = sys->accBuffer[j];
		}
	}

	// 计算均值
	mean = sum / PATTERN_RECOGNITION_LEN;
	// 计算方差
	for (k = 0; k < PATTERN_RECOGNITION_LEN - 1; k++) {
		std += (sys->accBuffer[k] - mean) * (sys->accBuffer[k] - mean) / PATTERN_RECOGNITION_LEN;
	}
	std = (float)sqrt(std);

	// 计算最大最小差值
	max_min = max - min;

	if (std > 5 && max_min > 25) {
		sys->type = 1;
	}
	else {
		sys->type = 0;
	}
}

/**----------------------------------------------------------------------
* Function    : calPredAngle
* Description : 根据pdr->cal_direction与gps_yaw角度综合确定角度angle
* Date        : 2020/7/22 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
double calPredAngle(PDR *g_pdr, classifer *sys, double gpsYaw, double trackYaw, imu *ss_data) {
	int attitude_flag = PDR_FALSE;
	g_detector = pdr_getDetector();

	/*姿态角YAW辅助计算start*/
	if (DETECTOR_TYPE_HANDHELD == g_detector->type)
	{
		b_timestamp = ss_data->gyr.time;
		if (!(angle_mean[0] == 0 && angle_mean[1] == 0 && angle_mean[2] == 0))
		{
			if (b_last_time > 0 && b_timestamp - b_last_time >= 5000 && gpsYaw != 0.0 && gpsYaw != -1 && trackYaw != -1)
			{
				/* 将姿态yaw角调整到0到2*PI */

				/*左臂包，头朝上，屏朝外 or 右臂包，头朝上，屏朝内*/
				if (ss_data->acc.s[1] > 9 && (fabs(angle_mean[2] + 6.15 - gpsYaw) < PI / 18 && fabs(angle_mean[2] + 6.15 - trackYaw) < PI / 18))
				{
					FUR_duration = -1;
					if (BUL_duration < 0)
					{
						BUL_duration = ss_data->gyr.time;
					}
					if (BUL_duration >= 0 && (b_timestamp - BUL_duration) >= 1500)
					{
						attitude_yaw = angle_mean[2] + 6.15 + PI;
						attitude_flag = PDR_TRUE;
						hold_type = BACKWARD_UP_LEFT;
					}
				}

				/*右臂包，头朝上，屏朝外 or 左臂包，头朝上，屏朝内*/
				if (ss_data->acc.s[1] > 9 && (fabs(angle_mean[2] + 7.10 - gpsYaw) < PI / 18 && fabs(angle_mean[2] + 7.10 - trackYaw) < PI / 18))
				{
					BUL_duration = -1;
					if (FUR_duration < 0)
					{
						FUR_duration = ss_data->gyr.time;
					}
					if (FUR_duration >= 0 && (b_timestamp - FUR_duration) >= 1500)
					{
						attitude_yaw = angle_mean[2] + 7.10;
						attitude_flag = PDR_TRUE;
						hold_type = FORWARD_UP_RIGHT;
					}
				}
			}
			if (-1 == b_last_time)
			{
				b_last_time = b_timestamp;
			}
		}
	}
	else{
		b_last_time = -1;
		FUR_duration = -1;
		BUL_duration = -1;
	}
	/*姿态角YAW辅助计算end*/

	double angle = -1;
	double ahrsYaw = atan2(g_pdr->cal_direction[1], g_pdr->cal_direction[0]);

	//printf("PCA Yaw = %f\n", ahrsYaw * (180 / 3.1415926));

	if (ahrsYaw < 0) {
		ahrsYaw += TWO_PI;
	}
	
	// 评估PCA计算方向角的准确性
	if ((g_pdr->bias_accuracy > 0 && g_pdr->bias_accuracy < 0.9) && (g_pdr->pca_accuracy > 0 && g_pdr->pca_accuracy < 0.25)) {
		angle = ahrsYaw;
		//printf("PCA准确性满足条件, angle = ahrsYaw %f\n", ahrsYaw * (180 / 3.1415926));
	}
	if (gpsYaw != 0.0 && gpsYaw != -1 ) //&& scene_type
	{
		// Status -- Explosion
		if (fabs(ss_data->acc.s[0]) > 70 || fabs(ss_data->acc.s[1]) > 70 || fabs(ss_data->acc.s[2]) > 70 || fabs(ss_data->gyr.s[0]) > 30 || fabs(ss_data->gyr.s[1]) > 30 || fabs(ss_data->gyr.s[2]) > 30) {
			angle = gpsYaw;
			//printf("Explosion, angle = gpsYaw = %f\n", gps_yaw * (180 / 3.1415926));
		}
		// Status -- Running 

		if (g_pdr->motionFreq > 2.7 || (g_pdr->motionType != 3 && g_pdr->motionType != 2) || (sys->type == 1)) { //&& g_pdr->motion_type != 2
			angle = gpsYaw;
			//printf("Running, angle = gpsYaw = %f\n", gps_yaw * (180 / 3.1415926));
		}
	}

	if (trackYaw != -1 && ((g_pdr->bias_accuracy < 0 || g_pdr->bias_accuracy > 0.05) || (g_pdr->pca_accuracy < 0 || g_pdr->pca_accuracy > 0.05))) {
		angle = trackYaw;//gps_yaw: obtained from the datasheet;     G_yaw : calculated from the route
		if (attitude_flag == PDR_TRUE)
		{
			angle = attitude_yaw;
		}
	}

	if (angle < 0)
	{
		if (g_pdr->bias_accuracy > 0 && g_pdr->pca_accuracy > 0 && (g_pdr->bias_accuracy < 1.5 && g_pdr->pca_accuracy < 0.5))//g_pdr->bias_accuracy < 6
		{
			angle = ahrsYaw;
		}
		else if (attitude_flag == PDR_TRUE)
		{
			angle = attitude_yaw;
		}
		else if (gpsYaw != 0.0 && gpsYaw != -1)
		{
			angle = gpsYaw;
		}
		else //if (lastYaw > 0)
		{
			angle = -1;  //lastYaw;
		}
	}

	return angle;
}



double calGnssTrackHeading(double gpsPos[][3], gnss pgnss)
{
	int i;
	double lla[3] = { 0.0 };
	double ned1[3] = { 0.0 };
	double temp[2][HIST_GPS_NUM] = { { 0.0 } };
	double angle[HIST_GPS_NUM - 1] = { 0.0 };

	double ned2[3] = { 0.0 };
	double maxn = 0;
	double maxe = 0;
	double minn = 0;
	double mine = 0;
	double meanx = 0;
	double meany = 0;

	float error = 0;
	float sst = 0;
	float r2 = 0;

	double range = 0;
	double yaw = 0;
	double tempYaw = 0;
	double diffAngle = 0;

	double para[3] = { 0 };
	int flg = 0;

	lla[0] = pgnss.lat;
	lla[1] = pgnss.lon;

	for (i = 0; i < HIST_GPS_NUM - 1; i++)
	{
		gpsPos[i][0] = gpsPos[i + 1][0];
		gpsPos[i][1] = gpsPos[i + 1][1];
		gpsPos[i][2] = gpsPos[i + 1][2];
	}

	if ((lla[0] > 0) && (lla[1] > 0))
	{
		//经纬度WGS84转地心地固坐标系ECEF
		WGS842ECEF(lla, gpsPos[HIST_GPS_NUM - 1]);
	}
	else
	{
		gpsPos[HIST_GPS_NUM - 1][0] = 0;
		gpsPos[HIST_GPS_NUM - 1][1] = 0;
		gpsPos[HIST_GPS_NUM - 1][2] = 0;
		return -1;
	}
	if (pgnss.error > 40)
	{
		return -1;
	}

	for (i = 0; i < HIST_GPS_NUM - 1; i++)
	{
		if ((fabs(gpsPos[i][0]) < 1e-6) && (fabs(gpsPos[i][1]) < 1e-6) && (fabs(gpsPos[i][2]) < 1e-6))
		{
			return -1;
		}
	}
	//地心地固坐标系ECEF转东北天坐标系NED
	ecef2Ned(gpsPos[HIST_GPS_NUM - 1], lla, ned1);

	for (i = HIST_GPS_NUM - 2; i >= 0; i--) {

		ecef2Ned(gpsPos[i], lla, ned2);

		temp[0][i] = ned1[0] - ned2[0];
		temp[1][i] = ned1[1] - ned2[1];

		meanx += temp[0][i] / HIST_GPS_NUM;
		meany += temp[1][i] / HIST_GPS_NUM;

		if (maxn > temp[0][i])
		{
			maxn = temp[0][i];
		}
		if (minn < temp[0][i])
		{
			minn = temp[0][i];
		}
		if (maxe > temp[1][i])
		{
			maxe = temp[1][i];
		}
		if (mine < temp[1][i])
		{
			mine = temp[1][i];
		}

		angle[i] = atan2(temp[1][i], temp[0][i]);
		if (angle[i] < 0)
			angle[i] += TWO_PI;
	}
	flg = leastDistanceLinearFit(temp[0], temp[1], HIST_GPS_NUM, para);
	if (flg < 0)
		return -1;

	for (i = HIST_GPS_NUM - 1; i >= 0; i--) {
		error += (float)(vivopow(para[0] * temp[0][i] + para[1] * temp[1][i] + para[2], 2) / (vivopow(para[0], 2) + vivopow(para[1], 2)));
		sst += (float)(vivopow(meanx - temp[0][i], 2) + vivopow(meany - temp[1][i], 2));
	}

	if (sst > 0)
	{
		r2 = 1 - error / sst;
	}
	range = sqrt(vivopow(maxn - minn, 2) + vivopow(maxe - mine, 2));
	//printf("%f,%f,%f\n", r2, error, range);
	if (r2 < 0.95 || range < 1.5 || error > 3.0)
		return -1;

	yaw = atan(-para[0] / para[1]);
	modAngle(&yaw, 0, TWO_PI);

	tempYaw = meanAngle(angle, HIST_GPS_NUM - 1);
	modAngle(&tempYaw, 0, TWO_PI);

	diffAngle = tempYaw - yaw;
	modAngle(&diffAngle, -PI, PI);

	if (fabs(diffAngle) > PI / 2)
		yaw += PI;
	modAngle(&yaw, 0, TWO_PI);

	return yaw;
}

/**----------------------------------------------------------------------
* Function    : pdr_insPredict
* Description : PDR惯导位置更新,
* Date        : 2020/07/08 yuanlin_rjyb@vivo.com
*               2021/02/06 zhanli@vivo.com : 感觉计步器输出不够平均，有时候
*               会造成PDR前后位置不够均匀
*---------------------------------------------------------------------**/
void  pdr_insPredict(PDR *g_pdr, StepPredict *stepPredict, imu *ss_data, 
	gnss *pgnss, KfPara *kf) {
	int step;
	long deltaStep;
	double stepLength = 0.7;

	stepPredict->fnum++;
	stepPredict->fsum += g_pdr->motionFreq;

	ulong  steps     = g_pdr->steps;
	ulong  lastSteps = g_pdr->lastSteps;
	
	if (steps - lastSteps > 0 && g_pdr->motionFreq != 0.0) {
		deltaStep = steps - lastSteps - stepPredict->deltaStep;

		// 应该是防止计步器异常情况，一次计步特别多
		if (deltaStep > 5)deltaStep = 5;
			
		if (stepPredict->meanTime > 0)
		{
			step = (int)((ss_data->gyr.time - stepPredict->lastTime) / stepPredict->meanTime + 0.5);
			
			if (step > deltaStep && step <= 3)deltaStep = step;
		}
		if (lastSteps == 0)deltaStep = 0;

		if (g_pdr->heading > 0) {
			pdr_ekfStatePredict(kf, stepLength, g_pdr, deltaStep);
			g_pdr->fusionPdrFlg = PDR_TRUE;
		}
		calStepLastTime(stepPredict, ss_data->gyr.time, lastSteps, steps);
	}else{
		// 有时候，计步器可能考虑到稳定性暂时不更新步数，因此需要利用GNSS速度预测步数，达到
		// 惯导更新位置的目的
		int preStep = predictStep(stepPredict, ss_data->gyr.time, lastSteps, pgnss->vel); 
		if (g_pdr->heading > 0){
			pdr_ekfStatePredict(kf, stepLength, g_pdr, preStep);
			if(preStep > 0)g_pdr->fusionPdrFlg = PDR_TRUE;
		}
	}
}

/**----------------------------------------------------------------------
* Function    : resetOutputResult
* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数
*               2、GPS值赋值给result
* Date        : 2020/07/08 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
int resetOutputResult(gnss *pgnss, PDR* g_pdr, KfPara *kf, lct_fs *result) {

	double stepLength = 0.7;
	double ned[3] = { 0 };

	g_pdr->pllaInit[0] = pgnss->lat;
	g_pdr->pllaInit[1] = pgnss->lon;
	g_pdr->pllaInit[2] = 0;
	wgs842Ned(g_pdr->pllaInit, g_pdr->pllaInit, ned);

	g_pdr->x0 = ned[0];
	g_pdr->y0 = ned[1];

	kf->p_xk[0] = ned[0];
	kf->p_xk[1] = ned[1];
	kf->p_xk[2] = stepLength;
	// 重置输出GPS时，GPS的航向角不一定准
	kf->p_xk[3] = pgnss->yaw*RADIAN_PER_DEG;

	for (uchar i = 0; i < N; i++) {
		kf->xk[i] = kf->p_xk[i];
	}

	for (int i = 0; i < N; i++) {
		for (int l = 0; l < N; l++) {
			kf->q[i][l] = 0.0;
			kf->r[i][l] = 0.0;
		}
	}
	//printf("重置PDR滤波器，输出GPS位置\n");
	// 输出GPS位置结果
	pdr_outputGpsPos(pgnss, result);
	g_pdr->noGpsCount = 0;
	return PDR_TRUE;
}

int pdr_initProc(gnss *pgnss, KfPara *kf, PDR* g_pdr,lct_fs *result) 
{
	double stepLen = 0.7;      // 初始运动步长为0.7m
	if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0  && pgnss->vel >= 1.5  && pgnss->yaw != -1) {
		// plla 类似位置坐标的原点(单位是经纬度)

		g_pdr->pllaInit[0] = pgnss->lat;
		g_pdr->pllaInit[1] = pgnss->lon;
		g_pdr->pllaInit[2] = 0;

		// plla 转 ned
		double ned[3] = { 0 };
		wgs842Ned(g_pdr->pllaInit, g_pdr->pllaInit, ned);

		// 在NED坐标系的计算, 单位是米
		g_pdr->x0 = ned[0];
		g_pdr->y0 = ned[1];

		// 初始化卡尔曼滤波器的观测量
		kf->p_xk[0] = ned[0];                        // 状态量1: 北向x
		kf->p_xk[1] = ned[1];                        // 状态量2：东向y
		kf->p_xk[2] = stepLen;                       // 状态量3：步长
		kf->p_xk[3] = pgnss->yaw * RADIAN_PER_DEG;   // 状态量4：航向角

		for (uchar i = 0; i < N; i++) {
			kf->xk[i] = kf->p_xk[i];
		}

		kf->initHeading = kf->p_xk[3];
		for (int i = 0; i < N; i++) {
			for (int l = 0; l < N; l++) {
				kf->q[i][l] = 0.0;
				kf->r[i][l] = 0.0;
			}
		}
		pdr_outputGpsPos(pgnss, result);
		g_pdr->sysStatus = IS_NORMAL;

		return PDR_TRUE;
	}

	if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel > 0 && pgnss->yaw != -1) {
		pdr_outputGpsPos(pgnss, result);
		return PDR_TRUE;
	}
	return PDR_FALSE;
}

/**----------------------------------------------------------------------
* Function    : calEkfQRMatrix
* Description : 根据GPS信号特征调整卡尔曼滤波噪声矩阵，q矩阵是过程噪声矩阵，
*               需要跟惯导预测位置精度相关。r矩阵为GNSS观测噪声，跟GPS输出的
*               信息精度有关。
* Date        : 2020/07/09 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void calEkfQRMatrix(lct_nmea *nmea, PDR *g_pdr, classifer *sys,
	KfPara *kf) {

	if (g_pdr->motionType != PDR_MOTION_TYPE_HANDHELD && 
		g_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((g_pdr->gpsHeading - g_pdr->lastGpsHeading))) > 50 ||
		g_pdr->gpsSpeed < 0.7 || g_pdr->gpsHeading == -1) {
		kf->r[3][3] = 10000;
	}
}
/**----------------------------------------------------------------------
* Function    : pdr_gnssInsLocFusion
* Description : PDR的GNSS和INS融合定位
* Date        : 2020/07/09 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void pdr_gnssInsLocFusion(gnss *pgnss, PDR *g_pdr, classifer *sys, double yaw_bias,
	KfPara *kf, lct_fs *res) {
	double plla[3] = { 0 };
	double ned[3] = { 0 };
	double yaw_thr = 6;

	int scene_type = g_pdr->sceneType;

	plla[0] = pgnss->lat;
	plla[1] = pgnss->lon;
	plla[2] = 0;

	wgs842Ned(plla, g_pdr->pllaInit, ned);

	pgnss->xNed = ned[0];
	pgnss->yNed = ned[1];

	//调整融合策略
	pdr_ekfStateUpdate(kf, pgnss, sys, g_pdr, scene_type);

	for (uchar i = 0; i < N; i++) {
		for (uchar l = 0; l < N; l++) {
			kf->p_pk[i][l] = kf->pk[i][l];
		}
		kf->p_xk[i] = kf->xk[i];
	}

	ned[0] = kf -> p_xk[0] - g_pdr->x0;
	ned[1] = kf -> p_xk[1] - g_pdr->y0;
	ned[2] = 0;

	ned2Wgs84(g_pdr->pllaInit, ned, plla);

	res->latitude        = plla[0];
	res->latitude_ns     = pgnss->lat_ns;
	res->longitudinal    = plla[1];
	res->longitudinal_ew = pgnss->lon_ew;
	res->time            = pgnss->timestamps;

	g_pdr->noGpsCount = 0;
}
