/******************** (C) COPYRIGHT 2020 Geek************************************ * File Name : PDRBase.c * Current Version : V2.0 * Author : logzhan * Date of Issued : 2022.10.15 * Comments : PDR基础功能函数 ********************************************************************************/ #include #include "PDRBase.h" #include "Location.h" #include "LinearFit.h" #include "Utils.h" #include "CoordTrans.h" #include "Kalman.h" extern double GpsPos[HIST_GPS_NUM][3]; 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; } double CalGnssTrackHeading(double gpsPos[][3], GNSS_t 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]; } // 经纬度WGS84转地心地固坐标系ECEF if ((lla[0] > 0) && (lla[1] > 0)){ 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)(pow(para[0] * temp[0][i] + para[1] * temp[1][i] + para[2], 2) / (pow(para[0], 2) + pow(para[1], 2))); sst += (float)(pow(meanx - temp[0][i], 2) + pow(meany - temp[1][i], 2)); } if (sst > 0) { r2 = 1 - error / sst; } range = sqrt(pow(maxn - minn, 2) + pow(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 : StateRecognition * Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳 * Date : 2022/09/19 logzhan *---------------------------------------------------------------------**/ void StateRecognition(float* acc, Classifer_t* sys) { } 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; } /**---------------------------------------------------------------------- * Function : InsLocationPredict * Description : PDR惯导位置更新 * Date : 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候 * 会造成PDR前后位置不够均匀 *---------------------------------------------------------------------**/ void InsLocationPredict(PDR_t* pdr, StepPredict* stepPredict, IMU_t* imu, GNSS_t* pgnss, EKFPara_t* kf) { int step; long deltaStep; double stepLength = 0.7; stepPredict->fnum++; stepPredict->fsum += pdr->motionFreq; ulong steps = pdr->steps; ulong lastSteps = pdr->lastSteps; if (steps - lastSteps > 0 && pdr->motionFreq != 0.0) { deltaStep = steps - lastSteps - stepPredict->deltaStep; // 应该是防止计步器异常情况，一次计步特别多 if (deltaStep > 5)deltaStep = 5; if (stepPredict->meanTime > 0) { step = (int)((imu->gyr.time - stepPredict->lastTime) / stepPredict->meanTime + 0.5); if (step > deltaStep && step <= 3)deltaStep = step; } if (lastSteps == 0)deltaStep = 0; if (pdr->heading > 0) { EKFStatePredict(kf, stepLength, pdr, deltaStep); pdr->fusionPdrFlg = PDR_TRUE; } calStepLastTime(stepPredict, imu->gyr.time, lastSteps, steps); } else { // 有时候，计步器可能考虑到稳定性暂时不更新步数，因此需要利用GNSS速度预测步数，达到 // 惯导更新位置的目的 int PreStep = PredictStep(stepPredict, imu->gyr.time, lastSteps, pgnss->vel); if (pdr->heading > 0) { EKFStatePredict(kf, stepLength, pdr, PreStep); if (PreStep > 0)pdr->fusionPdrFlg = PDR_TRUE; } } }