389 lines
13 KiB
C
389 lines
13 KiB
C
/******************** (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 <stdio.h>
|
||
#include <math.h>
|
||
#include <string.h>
|
||
#include <malloc.h>
|
||
#include <errno.h>
|
||
#include <stdlib.h>
|
||
#include <stdbool.h>
|
||
#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);
|
||
}
|