logzhan
/
PDR
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
PDR/3.BackUp/PDR-Origin-Version/include/pdr_base.h

343 lines
15 KiB
C
Raw Permalink Blame History

This file contains ambiguous Unicode characters!

This file contains ambiguous Unicode characters that may be confused with others in your current locale. If your use case is intentional and legitimate, you can safely ignore this warning. Use the Escape button to highlight these characters.

/******************** (C) COPYRIGHT 2021 VIVO************************************
* File Name : pdr_base.h
* Department : Sensor Algorithm Team
* Current Version : V2.0
* Author : yuanlin@vivocom & zhanli@vivo.com
* 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;
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 noGpsCount; // 没有GPS次数
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* pdr_initBase(void);
/**----------------------------------------------------------------------
* Function : pdr_resetData
* Description : 重置PDR变量
* Date : 2020/7/21 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void pdr_resetData(void);
/**----------------------------------------------------------------------
* Function : pdr_computePCA
* Description : 进入RESET_PHASE_1\STABLE阶段之后进行PCA计算
* Date : 2020/7/21 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void pdr_computePCA(AHRS* ahrs);
/**---------------------------------------------------------------------
* Function : detectorUpdate
* Description : 根据新确定的手机携带方式更新g_pdr.status等
* Date : 2020/7/20 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void detectorUpdate(DETECTOR* detector);
void gpsUpdateCb(gnss* 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 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
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 *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 pdr_insPredict(PDR *g_pdr, StepPredict *stepPredict,imu *ss_data,
gnss *pgnss, KfPara *kf);
/**----------------------------------------------------------------------
* Function : pdr_detStatic
* Description : 检测用户是否处于静止状态
* Date : 2021/01/28 zhanli@vivo.com
*---------------------------------------------------------------------**/
int pdr_detStatic(PDR *g_pdr, gnss *pgnss, unsigned long delSteps);
/**----------------------------------------------------------------------
* Function : pdr_outputGpsPos
* Description : 输出GPS位置
* Date : 2020/07/08 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
void pdr_outputGpsPos(gnss *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 yuanlin_rjyb@vivo.com
*---------------------------------------------------------------------**/
int resetOutputResult(gnss *pgnss, PDR* g_pdr, KfPara *kf, lct_fs *result);
/**----------------------------------------------------------------------
* Function : detIsCarMode
* Description : 识别是否是车载模式
* Input : pgnss，GPS数据结构体
g_pdr，PDR结构体
delSteps， 与上次相比步数的变化量
time，计数器
* Output : int，车载模式标志位
* Date : 2021/01/28 zhanli@vivo.com
*---------------------------------------------------------------------**/
int detIsCarMode(gnss *pgnss, PDR *g_pdr, unsigned long delSteps, int *time);
/**----------------------------------------------------------------------
* Function : detPdrToReset
* Description : 检测PDR系统是否需要重置，考虑到后续PDR推算失误，或者其他情况
* 重置PDR到GPS的位置。
* Date : 2021/01/28 zhanli@vivo.com
*---------------------------------------------------------------------**/
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 pdr_initProc(gnss *pgnss, KfPara *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 pgnss);
#endif
#pragma once
Reference in New Issue View Git Blame Copy Permalink