347 lines
15 KiB
C
347 lines
15 KiB
C
/******************** (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
|