/******************** (C) COPYRIGHT 2020 Geek************************************ * File Name : pdr_location.c * Current Version : V1.2 * Author : logzhan * Date of Issued : 2020.7.3 * Comments : PDR导航算法主流程 ********************************************************************************/ /* Header File Including -----------------------------------------------------*/ #include #include #include #include #include #include #include #include "Location.h" #include "Kalman.h" #include "Pedometer.h" #include "AHRS.h" #include "pdr_detector.h" #include "Utils.h" #include "scene_recognition.h" #include "CoordTrans.h" /* Global Variable Definition ------------------------------------------------*/ PDR_t* pdr; // PDR全局信息 StepPredict stepPredict = { 0 }; classifer sys; GNSS_t pgnss; // 定位相关信息 EKFPara_t g_kfPara; double GpsPos[HIST_GPS_NUM][3] = {{0}}; static uint32_t g_status; extern AHRS_t g_ahrs; double refGpsYaw = -1000; double yaw_bias = -1; float dir[2] = { 0 }; float bias_dir[2] = { 0 }; /**--------------------------------------------------------------------- * Function : PDRNav_Init * Description : PDR导航系统初始化 * Date : 2022/09/21 logzhan *---------------------------------------------------------------------**/ void PDRNav_Init(void) { memset(&g_kfPara, 0, sizeof(g_kfPara)); memset(&pgnss, 0, sizeof(pgnss)); memset(GpsPos, 0, sizeof(GpsPos)); memset(&stepPredict, 0, sizeof(stepPredict)); memset(&sys, 0, sizeof(sys)); memset(dir, 0, sizeof(dir)); memset(bias_dir, 0, sizeof(bias_dir)); // GNSS定位初始化 InitGnssInfo(); // 初始化AHRS姿态解算相关 AHRS_Init(); // 检测器初始化 Detector_Init(); // PDR初始化 pdr = Base_Init(); //base文件中姿态角计算初始化 EKF_Init(); // 场景识别模块初始化 SceneRecog_Init(); yaw_bias = -1; } /**--------------------------------------------------------------------- * Function : InitGnssInfo * Description : PDS GNSS定位信息初始化 * Date : 2020/02/01 logzhan: * 创建初始版本 * 2020/02/08: logzhan 修改initGps为initGnssInfo，更 * 加符合函数功能 *---------------------------------------------------------------------**/ void InitGnssInfo(void) { pgnss.lastError = ACCURACY_ERR_MAX; pgnss.overVelCount = 0; } /**--------------------------------------------------------------------- * Function : ResetSystemStatus * Description : PDR 重设系统状态 * Date : 2022/09/16 logzhan *---------------------------------------------------------------------**/ void ResetSystemStatus(EKFPara_t *kf) { memset(kf, 0, sizeof(EKFPara_t)); pdr->fusionPdrFlg = PDR_FALSE; pdr->sysStatus = IS_INITIAL; pgnss.overVelCount = 0; } /**--------------------------------------------------------------------- * Function : InsLocation * Description : PDR 惯性导航定位 * Date : 2022/09/16 logzhan *---------------------------------------------------------------------**/ int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) { // AHRS姿态更新占2%的运行时间 if (UpdateAHRS(ImuData)) { // 完成姿态估计后计算PCA， 占26%的运行时间 ComputePCA(&g_ahrs); } // 更新用户运动类型检测器IMU信息以及用户运动类型检测, 占21%的运行时间 DetectorUpdateIMU(ImuData); pdr->gyroTime = ImuData->gyr.time; // 为什么手持类型是0.1的运动频率？ if (pdr->motionType == DETECTOR_TYPE_HANDHELD)pdr->motionFreq = 0.1f; // 计步器输入传感器数据，占10%的运行时间 PedometerUpdate(ImuData, &pdr->steps); // ----- Filter Initialization ------- // if (pdr->sysStatus == IS_INITIAL) { pdr->ts = ImuData->gyr.time; pdr->lastSteps = pdr->steps; stepPredict.lastTime = 0; stepPredict.fnum = 0; stepPredict.fsum = 0; return 0; } pdr->ts = ImuData->gyr.time; if ((0 == ImuData->acc.s[0] && 0 == ImuData->acc.s[1] && 0 == ImuData->acc.s[2]) || (0 == ImuData->mag.s[0] && 0 == ImuData->mag.s[1] && 0 == ImuData->mag.s[2])) { return 0; } // 识别当前状态是否平稳，这个函数计算量到达整个系统10%的计算量, 以后根据情况进行优化 StateRecognition(ImuData->acc.s, &sys); //pdr->heading = CalPredHeading(...); double imuYaw = - g_ahrs.yaw; if (imuYaw < 0) { imuYaw += 360; } pdr->heading = imuYaw / (180 / 3.14159265); if (fabs(pdr->insHeadingOffset) > 0.000001) { imuYaw = imuYaw - pdr->insHeadingOffset * (180 / 3.14159265); pdr->heading = imuYaw / (180 / 3.14159265); } // PDR 惯性位置预测 InsLocationPredict(pdr, &stepPredict, ImuData, &pgnss, Ekf); pdr->lastSteps = pdr->steps; pdr->lastHeading = pdr->heading; return 0; } /**---------------------------------------------------------------------- * Function : GnssInsFusionLocation * Description : PDR GPS融合INS惯性定位 * Date : 2021/01/29 logzhan *---------------------------------------------------------------------**/ int GnssInsFusionLocation(lct_nmea *nmea_data, EKFPara_t *kf, lct_fs *result) { // GPS数据更新 GnssUpdate(&pgnss, nmea_data); // 检查时间差是否满足条件 if (fabs(pgnss.timestamps - pdr->ts) >= 1000)return TYPE_FIX_NONE; // 根据nmea数据识别场景是否是开阔天空场景 pdr->sceneType = isOpenArea(nmea_data); // 根据HDOP、速度、卫星数量等判断gps yaw是否可用 pdr->gpsSpeed = pgnss.vel * GPS_SPEED_UNIT; pdr->lastGpsHeading = pdr->gpsHeading; pdr->gpsHeading = GnssCalHeading(&pgnss); // 如果在win32平台下仿真调试,则添加下面代码 #ifdef _WIN32 result->pdrHeading = kf->pXk[3]; result->gpsHeading = pdr->gpsHeading; result->hdop = pgnss.HDOP; result->accuracy = nmea_data->accuracy.err; result->gpsSpeed = pdr->gpsSpeed; result->motionType = pdr->motionType; #endif CalPdrHeadingOffset(nmea_data, pdr); // 计算两次GPS时间之间，步数变化量 pdr->deltaStepsPerGps = pdr->steps - pdr->lastGpsSteps; pdr->lastGpsSteps = pdr->steps; int mode = DetectFixMode(&pgnss, kf, pdr, result); if (mode != TYPE_FIX_PDR)return mode; //根据历史GPS拟合输出预估的GPS轨迹航向角 pdr->trackHeading = CalGnssTrackHeading(GpsPos, pgnss); // 如果系统没有初始化，则先初始化 if (pdr->sysStatus == IS_INITIAL) { return InitProc(&pgnss, kf, pdr, result); } // 如果惯导没有达到输出条件，则不预测轨迹 if (!pdr->fusionPdrFlg)return ResetOutputResult(&pgnss, pdr, kf, result); // 根据GPS信息, 计算kf的过程噪声和观测噪声 EKFCalQRMatrix(nmea_data, pdr, &sys, kf); if (pgnss.satellites_num > 4){ // 如果GPS航向角处于无效状态且速度也是无效状态 if (pgnss.yaw == INVAILD_GPS_YAW || pgnss.vel <= 0.0) { //if(pdr->heading != INVAILD_GPS_YAW){ // pgnss.yaw = (float)(R2D(pdr->heading)); //}else if (pdr->trackHeading != INVAILD_GPS_YAW) { // pgnss.yaw = (float)(R2D(pdr->trackHeading)); //} pgnss.yaw = (float)kf->pXk[3]; } if (pgnss.yaw != INVAILD_GPS_YAW) { GnssInsLocFusion(&pgnss, pdr, &sys, yaw_bias, kf, result); return TYPE_FIX_PDR; } } // 不满足推算条件，直接输出原始GPS if (pgnss.satellites_num > 4 && nmea_data->accuracy.err > 0 && nmea_data->accuracy.err < 15) { OutputOriginGnssPos(&pgnss, result); pdr->NoGnssCount = 0; return TYPE_FIX_GPS; } // 如果没有GPS信号，那么PDR也会接着推算位置, 但是最多推算10个点 if (pdr->NoGnssCount < MAX_NO_GPS_PREDICT) { // 无GPS状态位置推算 NoGnssInfoPredict(kf, result, pdr); return TYPE_FIX_PDR; } pdr->NoGnssCount++; return 0; } /**---------------------------------------------------------------------- * Function : NoGnssInfoPredict * Description : 在没有gps信息时预测GPS位置 * Date : 2022/09/16 *---------------------------------------------------------------------**/ void NoGnssInfoPredict(EKFPara_t* kf, lct_fs* result, PDR_t* p_pdr) { double ned[3] = {0}, lla[3] = {0}; ned[0] = kf->pXk[0] - p_pdr->x0; ned[1] = kf->pXk[1] - p_pdr->y0; ned[2] = 0; NED2WGS84(p_pdr->pllaInit, ned, lla); result->latitude = lla[0]; result->latitude_ns = pgnss.lat_ns; result->longitudinal = lla[1]; result->longitudinal_ew = pgnss.lon_ew; result->time = pgnss.timestamps; p_pdr->NoGnssCount++; } /**---------------------------------------------------------------------- * Function : calPdrHeadingOffset * Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移 * Date : 2020/07/08 logzhan *---------------------------------------------------------------------**/ void CalPdrHeadingOffset(lct_nmea* nmea_data, PDR_t* p_pdr) { //惯导姿态角与GPS行进角度的对准（判断GPS信号是否正常以及惯导是否平稳） if (refGpsYaw != -1000)return; if ((pgnss.satellites_num > 4 && pgnss.HDOP < 2.0 && pgnss.yaw != -1 && pgnss.vel >= 1.0) && (nmea_data->accuracy.err > 0 && nmea_data->accuracy.err < 4) && (pdr_detStatic(pdr, &pgnss, (p_pdr->steps - p_pdr->lastSteps)) == 0 && pdr->motionType == 2)) { /*这里放姿态角与GPS对准part1 start*/ double imuHeading = atan2(2 * (g_ahrs.qnb[0] * g_ahrs.qnb[3] + g_ahrs.qnb[1] * g_ahrs.qnb[2]), 1 - 2 * (g_ahrs.qnb[2] * g_ahrs.qnb[2] + g_ahrs.qnb[3] * g_ahrs.qnb[3])); if (imuHeading < 0) { imuHeading += TWO_PI; } if (fabs(imuHeading - p_pdr->gpsHeading) < PI / 18) { //printf("offset 标定\n"); //p_pdr->insHeadingOffset = imuHeading - p_pdr->gpsHeading; } } } /**---------------------------------------------------------------------- * Function : pdr_detectFixMode * Description : 检测当前PDR处于的模式，如果是车载和静止模式,根据情况选择输出 * 原始GPS或者不输出 * Date : 2020/07/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高 * 代码的可读性 *---------------------------------------------------------------------**/ int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, lct_fs* result) { // 检测用户是否处于静止状态 if (pdr_detStatic(pdr, pgnss, g_pdr->deltaStepsPerGps)) { if (pgnss->error > 0 && pgnss->error < 15 && pgnss->lastError >= pgnss->error) { OutputOriginGnssPos(pgnss, result); pgnss->lastError = pgnss->error; return TYPE_FIX_PDR; } // 否则不输出位置 return TYPE_FIX_NONE; } // 如果是检测到是开车状态则输出GPS位置 if (DetectCarMode(pgnss, pdr, pdr->deltaStepsPerGps, &pgnss->overVelCount)) { // 如果检测到车载模式，则重置PDR状态，需要重新初始化 ResetSystemStatus(kf); if (pgnss->error >= 0 || (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0)) { OutputOriginGnssPos(pgnss, result); return TYPE_FIX_GPS; } // 精度过差，不输出位置 return TYPE_FIX_NONE; } return TYPE_FIX_PDR; } /**---------------------------------------------------------------------- * Function : GnssCalHeading * Description : 获取GPS偏航角(0 - 360°) * Date : 2022/09/16 logzhan * 代码的可读性 *---------------------------------------------------------------------**/ double GnssCalHeading(GNSS_t* pgnss) { double gnssHeading = INVAILD_GPS_YAW; if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.0 && pgnss->yaw != INVAILD_GPS_YAW && pgnss->error < 50) { gnssHeading = pgnss->yaw * RADIAN_PER_DEG; } return gnssHeading; } /**---------------------------------------------------------------------- * Function : pdr_nmea2Gnss * Description : nmea数据结构转gnss数据 * Date : 2020/07/08 logzhan *---------------------------------------------------------------------**/ void Nmea2Gnss(lct_nmea* nmea_data, GNSS_t* pgnss) { lct_nmea_rmc rmc = { 0 }; if (nmea_data->rmc[0].time > nmea_data->rmc[1].time) { rmc = nmea_data->rmc[0]; }else { rmc = nmea_data->rmc[1]; } // 更新上一次的Accuracy Error pgnss->lastError = pgnss->error; pgnss->lat = rmc.latitude * PI / 180; pgnss->lat_ns = rmc.rmc_latitude_ns; pgnss->lon = rmc.longitudinal * PI / 180; pgnss->lon_ew = rmc.longitudinal_ew; //pgnss->vel = rmc.speed * GPS_SPEED_UNIT; pgnss->vel = rmc.speed; pgnss->HDOP = nmea_data->gsa[0].hdop; pgnss->yaw = rmc.cog; pgnss->quality = 0; pgnss->error = nmea_data->accuracy.err; pgnss->timestamps = rmc.time; pgnss->satellites_num = nmea_data->gga.satellites_nb; } /**---------------------------------------------------------------------- * Function : GnssUpdate * Description : nmea数据结构转gnss数据 * Date : 2020/07/08 logzhan * 2020/02/09 logzhan : 函数对应原pdr_gpsUpdate函数， * 经过引用分析发现每次GPS更新拷贝gnss结构体和nmea结构体其实没 * 什么用，还增加运算量 *---------------------------------------------------------------------**/ void GnssUpdate(GNSS_t* gps, lct_nmea* nmea) { // nmea数据转gnss数据结构 Nmea2Gnss(nmea, &pgnss); // GPS回调更新，暂时不清楚函数功能 gpsUpdateCb(gps); }