/******************** (C) COPYRIGHT 2021 Geek************************************ * File Name : pdr_base.h * Department : Sensor Algorithm Team * Current Version : V2.0 * Author : yuanlin@vivocom & * * * Date of Issued : 2021.02.01 * Comments : PDR 算法基本定义头文件 ********************************************************************************/ #ifndef _PDR_BASE_H_ #define _PDR_BASE_H_ /* Header File Including -----------------------------------------------------*/ #include "stdint.h" #include "pdr_sensor.h" /* PDR SYS CFG ---------------------------------------------------------------*/ #define PDR_OUTPUT_SMOOTH 0 // 是否开启轨迹平滑 /* Macro Definition ----------------------------------------------------------*/ #define PCA_NUM 200 #define ACCURACY_ERR_MAX 1000 // GPS的accuracy最大值,一般用于初始化用 #define N 4 // 矩阵维数 #define OPEN_SKY 1 #define MAX_NO_GPS_PREDICT 10 // 无GPS信息状态，最大位置推算数量 #define UN_UPDATE 0 // 非更新状态 #define DATA_UPDATE 1 // 数据更新 #define ON 1 #define OFF 0 #define PDR_TRUE 1 #define PDR_FALSE 0 #define TYPE_FIX_NONE 0 // PDR修正结果为不输出 #define TYPE_FIX_GPS 2 // PDR修正结果为输出原始GPS #define TYPE_FIX_PDR 1 // PDR修正结果为PDR融合 // PDR携带方式 #define UNKNOWN 0 #define FORWARD_UP_RIGHT 1 // 臂包：右臂，头朝上，屏朝外；or左臂，头朝上，屏朝内 #define BACKWARD_UP_LEFT 2 // 臂包：左臂，头朝上，屏朝外；or右臂，头朝上，屏朝内 #define BACKWARD_DOWN_RIGHT 3 // 臂包：右臂，头朝下，屏朝外；or左臂，头朝下，屏朝内 #define FORWARD_DOWN_LEFT 4 // 臂包：左臂，头朝下，屏朝外；or右臂，头朝下，屏朝内 #define LEFT_UP_FORWARD 5 // 裤兜：头朝上，屏朝前 #define RIGHT_UP_BACKWARD 6 // 裤兜：头朝上，屏朝后 #define RIGHT_DOWN_FORWARD 7 // 裤兜：头朝下，屏朝前 #define LEFT_DOWN_BACKWARD 8 // 裤兜：头朝下，屏朝后 // PDR相关状态 #define PDR_NO_ERROR 0 #define PDR_OUT_OF_MEMORY 1 #define PDR_MOTION_TYPE_STATIC 0 #define PDR_MOTION_TYPE_IRREGULAR 1 #define PDR_MOTION_TYPE_HANDHELD 2 #define PDR_MOTION_TYPE_SWINGING 3 #define PDR_STATUS_RESET 0x80 #define PDR_STATUS_CACULATING_AXIS_CEOFFICIENT 0x40 #define PDR_STATUS_PCA_STABLE 0x20 #define PDR_STATUS_BIAS_STABLE 0x10 #define PDR_STATUS_STABLE (PDR_STATUS_PCA_STABLE | PDR_STATUS_BIAS_STABLE) // 地球相关常数 #define WGS84_RE 6378137 // WGS-84椭球体长半轴 #define WGS84_ECCENTR2 6.69437999014e-3 // 第一偏向率的平方，e^2(WGS-84) #define WGS84_OMEGA_E_DOT 7.2921151467e-5 // 地球自转角速度 rad/sec #define WGS84_GRAVITY 9.7803267714 // 地球平均重力加速度 m/s^2 #define DEG_PER_RADIAN 57.295779513082323 // degrees per radian #define RADIAN_PER_DEG 0.017453292519943 // radians per degree #define GPS_SPEED_UNIT 0.5144444 // GPS原始速度转m/s的缩放系数 #define PI 3.1415926 #define DOUBLE_ZERO 1e-10 #define TWO_PI (2.0*PI) #define R2D(x) (x*57.2957795130823) #define D2R(x) (x*3.14159265/180.0) #define USE_BUG_FOR_LOCAL_PARA 1 #define BUF_DMT_1 1 #define BUF_DMT_2 2 #define PRINT_ALGO_RUN_TIME 0 #define PRINT_CLCT 1 #define PATTERN_RECOGNITION_LEN 256 // 缓存256 / SAMPLE_RATE的数量 #define OPEN_AREA 1 // 开阔区域(GPS信号较好) #define UN_OPEN_AREA 0 // 非开阔区域(GPS信号较弱) #define DETECTOR_RUN_FREQ 1280 // 用户运动类型检测器检测周期ms #define DETECTOR_TYPE_STATIC 0 // 用户静止 #define DETECTOR_TYPE_IRREGULAR 1 // 无规律运动 #define DETECTOR_TYPE_HANDHELD 2 // 手持运动 #define DETECTOR_TYPE_SWINGING 3 // 摆手运动 #define ulong unsigned long #define uchar unsigned char typedef enum { IS_INITIAL = 0, IS_NORMAL = 1, }pdrStatus; typedef struct { double xk[N]; // 状态变量Xk xk[0]: 北向x xk[1]：东向y xk[2]：步长 xk[3] ：航向角 double p_xk[N]; // 最佳预测变量Xk_predict xk[0]: 北向x xk[1]：东向y xk[2]：步长 xk[3] ：航向角 double zk[N]; double p_pk[N][N]; double pk[N][N]; double phi[N][N]; double hk[N][N]; double q[N][N]; // 卡尔曼滤波的Q矩阵(过程噪声) double r[N][N]; // 卡尔曼滤波R矩阵(观测噪声) double Kk[N][N]; double lambda; double plat; double plon; double initHeading; }KfPara_t; typedef struct{ int fnum; int deltaStep; float fsum; float meanTime; double lastTime; }StepPredict; typedef struct PDR { uint32_t status; // PDR当前状态 uint32_t motionType; // 用户运动类型 uint32_t NoGnssCount; // 没有GNSS信息次数 pdrStatus sysStatus; // PDR系统状态 int sceneType; // 场景类型：1：开阔区域，0：非开阔区域(信号较弱) int fusionPdrFlg; // 融合PDR位置标志位, 当flg为0时纯输出GPS double pllaInit[3]; // 初始plla坐标 double ts; // 时间戳 double x0; // 初始北向坐标 double y0; // 初始东向坐标 // 航向角相关 double heading; // PDR方向角 double lastHeading; // 上一次PDR方向角 double deltaHeading; // 偏航角变化量 double insHeadingOffset; // 惯导方向角偏移 double imuDeltaHeading; // 没间隔一次GPS信号，PDR变化的角度，用于区分转弯 double gpsHeading; // GPS航向角 double lastGpsHeading; // 上一次GPS航向角 double trackHeading; // GPS轨迹航向角 // 速度相关 double gpsSpeed; // GPS速度 // 步数相关 ulong steps; // 当前步数信息 ulong lastSteps; // 上一次的步数 ulong lastGpsSteps; // 上一次GPS步数 ulong deltaStepsPerGps; // 两次GPS更新之间，步数的变化量 float motionFreq; // 运动频率 double gyroTime; // 陀螺仪时间 float axis_ceofficient[3]; float axis_direction[2]; float pca_direction[2]; float pca_accuracy; float bias_direction[2]; float bias_accuracy; float cal_direction[2]; } PDR; typedef struct { int type; float accBuffer[PATTERN_RECOGNITION_LEN]; }classifer; // 用户运动类型分类器 typedef struct DETECTOR { uint32_t type; // 用户运动类别 ： 0:静止运动 1：无规律运动 2：手持运动 3：摆手运动 uint32_t lastType; uint64_t tick; // 次数统计，用于调整检测器工作频率 } DETECTOR; DETECTOR *pdr_getDetector(void); /* Function Declaration ------------------------------------------------------*/ PDR* Base_Init(void); /**---------------------------------------------------------------------- * Function : pdr_resetData * Description : 重置PDR变量 * Date : 2020/7/21 * *---------------------------------------------------------------------**/ void pdr_resetData(void); /**---------------------------------------------------------------------- * Function : pdr_computePCA * Description : 进入RESET_PHASE_1\STABLE阶段之后进行PCA计算 * Date : 2020/7/21 logzhan *---------------------------------------------------------------------**/ void ComputePCA(AHRS_t* ahrs); /**--------------------------------------------------------------------- * Function : detectorUpdate * Description : 根据新确定的手机携带方式更新g_pdr.status等 * Date : 2020/7/20 logzhan *---------------------------------------------------------------------**/ void detectorUpdate(DETECTOR* detector); void gpsUpdateCb(GNSS_t* gps); /************************************************************************** * Description : 计算每步所耗时间 * Input : stepPredict， 步数预测结构体 timestamp，时间戳 steps_last，上一次步数 steps，本次步数 **************************************************************************/ void calStepLastTime(StepPredict *stepPredict, double timestamp, unsigned long steps_last, unsigned long steps); /************************************************************************** * Description : 预测步数 * Input : stepPredict， 步数预测结构体 timestamp，时间戳 steps_last，上一次步数 gnssVel，gps速度 * Output : int， 步数 **************************************************************************/ int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_last, float gnssVel); /**---------------------------------------------------------------------- * Function : stateRecognition * Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳 * Date : 2020/7/22 logzhan *---------------------------------------------------------------------**/ void StateRecognition(float *acc, classifer *sys); /************************************************************************** * Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向 * Input : g_pdr，PDR结构体 sys，运动幅度数据结构体 gps_yaw，GPS轨迹角 G_yaw，GPS方向角 ss_data， 传感器数据结构体 * Output : double，用户行走方向 **************************************************************************/ double calPredAngle(PDR *g_pdr, classifer *sys, double gps_yaw, double G_yaw, IMU_t *ss_data); /************************************************************************** * Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向 * Input : g_pdr，PDR结构体 steps，本次步数 steps_last，上一次步数 stepPredict，步数预测结构体 pdr_angle，用户行走方向 scene_type，GPS用户所处场景（开阔和非开阔） ss_data， 传感器数据结构体 pgnss，GPS数据结构体 kf，EKF结构体 pdr, PDR预测位置成功标志位 **************************************************************************/ void InsLocationPredict(PDR *g_pdr, StepPredict *stepPredict,IMU_t *ss_data, GNSS_t *pgnss, KfPara_t *kf); /**---------------------------------------------------------------------- * Function : pdr_detStatic * Description : 检测用户是否处于静止状态 * Date : 2021/01/28 logzhan *---------------------------------------------------------------------**/ int pdr_detStatic(PDR *g_pdr, GNSS_t *pgnss, unsigned long delSteps); /**---------------------------------------------------------------------- * Function : pdr_outputGpsPos * Description : 输出GPS位置 * Date : 2020/07/08 logzhan *---------------------------------------------------------------------**/ void OutputOriginGnssPos(GNSS_t *pgnss, lct_fs *result); /**---------------------------------------------------------------------- * Function : resetOutputResult * Description : 1、初始化解算初始位置、卡尔曼滤波初始参数 * 2、GPS值赋值给result * pgnss，GPS数据结构体 * plla_init，待重置NED坐标系原点经纬度 * x_init，y_init待重置NED坐标系原点的n和e坐标 * kf， EKF数据结构体 * Date : 2020/07/08 logzhan *---------------------------------------------------------------------**/ int ResetOutputResult(GNSS_t *pgnss, PDR* g_pdr, KfPara_t *kf, lct_fs *result); /**---------------------------------------------------------------------- * Function : detIsCarMode * Description : 识别是否是车载模式 * Input : pgnss，GPS数据结构体 g_pdr，PDR结构体 delSteps， 与上次相比步数的变化量 time，计数器 * Output : int，车载模式标志位 * Date : 2021/01/28 logzhan *---------------------------------------------------------------------**/ int DetectCarMode(GNSS_t *pgnss, PDR *g_pdr, unsigned long delSteps, int *time); /**---------------------------------------------------------------------- * Function : detPdrToReset * Description : 检测PDR系统是否需要重置，考虑到后续PDR推算失误，或者其他情况 * 重置PDR到GPS的位置。 * Date : 2021/01/28 logzhan *---------------------------------------------------------------------**/ int detPdrToReset(double pdr_angle, int *gpscnt, unsigned long deltsteps, PDR *g_pdr); /************************************************************************** * Description : 初始化EKF滤波器 * Input : pgnss，GPS数据结构体 plla_init，NED坐标系原点经纬度 kf， EKF数据结构体 x_init，y_init，NED坐标系原点的n和e坐标 sys_status，定位系统状态（未初始化和已初始化） * Output : int，初始化标志位 result，定位结果结构体 **************************************************************************/ int InitProc(GNSS_t *pgnss, KfPara_t *kf, PDR* g_pdr, lct_fs *result); /************************************************************************** * Description : 设置EKF滤波器的Q和R * Input : scane_type, 用户所处场景（开阔和非开阔） nmea_data，NMEA数据结构体 g_pdr，PDR结构体 sys，运动幅度数据结构体 kf，EKF数据结构体 **************************************************************************/ /************************************************************************** * Description : 计算GPS轨迹角 * Input : gpsPos，GPS历史定位点在ECEF下的三维坐标 pgnss，GPS数据结构体 * Output : double，轨迹角 **************************************************************************/ double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss); #endif #pragma once