logzhan
/
PDR
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
PDR/1.Software/PDR 1.05/src/PDRBase.c

258 lines
6.5 KiB
C
Raw Normal View History

优化PDR框架构成，创建PDR1.05
2022-10-17 20:33:46 +08:00
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : PDRBase.c
* Current Version : V2.0
* Author : logzhan
* Date of Issued : 2022.10.15
* Comments : PDR<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܺ<EFBFBD><EFBFBD><EFBFBD>
********************************************************************************/
大幅度精简无用代码
2022-10-18 13:36:05 +08:00
#include <math.h>
优化PDR框架构成，创建PDR1.05
2022-10-17 20:33:46 +08:00
#include "PDRBase.h"
大幅度精简无用代码
2022-10-18 13:36:05 +08:00
#include "Location.h"
#include "LinearFit.h"
#include "Utils.h"
#include "CoordTrans.h"
#include "Kalman.h"
优化PDR框架构成，创建PDR1.05
2022-10-17 20:33:46 +08:00
大幅度精简无用代码
2022-10-18 13:36:05 +08:00
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];
}
// <20><>γ<EFBFBD><CEB3>WGS84ת<34><D7AA><EFBFBD>ĵع<C4B5><D8B9><EFBFBD><EFBFBD><EFBFBD>ϵ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;
}
}
//<2F><><EFBFBD>ĵع<C4B5><D8B9><EFBFBD><EFBFBD><EFBFBD>ϵECEFת<46><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ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 : <EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD><EFBFBD>ٶȼ<EFBFBD><EFBFBD>жϵ<EFBFBD>ǰ״̬<EFBFBD>Ƿ<EFBFBD>ƽ<EFBFBD><EFBFBD> 1: <EFBFBD><EFBFBD>ƽ<EFBFBD><EFBFBD> 0: ƽ<EFBFBD><EFBFBD>
* 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<EFBFBD>ߵ<EFBFBD>λ<EFBFBD>ø<EFBFBD><EFBFBD><EFBFBD>
* Date : 2021/02/06 logzhan : <EFBFBD>о<EFBFBD><EFBFBD>Ʋ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PDRǰ<EFBFBD><EFBFBD>λ<EFBFBD>ò<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*---------------------------------------------------------------------**/
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;
// Ӧ<><D3A6><EFBFBD>Ƿ<EFBFBD>ֹ<EFBFBD>Ʋ<EFBFBD><C6B2><EFBFBD><EFBFBD>쳣<EFBFBD><ECB3A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>μƲ<CEBC><C6B2>ر<EFBFBD><D8B1><EFBFBD>
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 {
// <20><>ʱ<EFBFBD>򣬼Ʋ<F2A3ACBC><C6B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܿ<EFBFBD><DCBF>ǵ<EFBFBD><C7B5>ȶ<EFBFBD><C8B6><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD>²<EFBFBD><C2B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>GNSS<53>ٶ<EFBFBD>Ԥ<EFBFBD>ⲽ<EFBFBD><E2B2BD><EFBFBD><EFBFBD><EFBFBD>ﵽ
// <20>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>õ<EFBFBD>Ŀ<EFBFBD><C4BF>
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;
}
}
}