258 lines
6.5 KiB
C
258 lines
6.5 KiB
C
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||
* File Name : PDRBase.c
|
||
* Current Version : V2.0
|
||
* Author : logzhan
|
||
* Date of Issued : 2022.10.15
|
||
* Comments : PDR»ù´¡¹¦Äܺ¯Êý
|
||
********************************************************************************/
|
||
#include <math.h>
|
||
#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;
|
||
}
|
||
}
|
||
} |