233 lines
10 KiB
C
233 lines
10 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 pXk[N]; // 最佳预测变量Xk_predict xk[0]: 北向x xk[1]：东向y xk[2]：步长 xk[3] ：航向角
|
||
double Zk[N];
|
||
double pPk[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;
|
||
}EKFPara_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 pcaAccuracy;
|
||
float bias_direction[2];
|
||
float biasAccuracy;
|
||
float cal_direction[2];
|
||
} PDR_t;
|
||
|
||
typedef struct {
|
||
int type;
|
||
float accBuffer[PATTERN_RECOGNITION_LEN];
|
||
}Classifer_t;
|
||
|
||
// 用户运动类型分类器
|
||
typedef struct DETECTOR {
|
||
uint32_t type; // 用户运动类别 ： 0:静止运动 1：无规律运动 2：手持运动 3：摆手运动
|
||
uint32_t lastType;
|
||
uint64_t tick; // 次数统计，用于调整检测器工作频率
|
||
}Detector_t;
|
||
|
||
|
||
/* Function Declaration ------------------------------------------------------*/
|
||
|
||
|
||
/**************************************************************************
|
||
* 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_t *sys);
|
||
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : InsLocationPredict
|
||
* Description : PDR惯导位置更新
|
||
* Date : 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候
|
||
* 会造成PDR前后位置不够均匀
|
||
*---------------------------------------------------------------------**/
|
||
void InsLocationPredict(PDR_t* pdr, StepPredict* stepPredict, IMU_t* imu,
|
||
GNSS_t* pgnss, EKFPara_t* kf);
|
||
|
||
|
||
|
||
/**************************************************************************
|
||
* Description : 计算GPS轨迹角
|
||
* Input : gpsPos，GPS历史定位点在ECEF下的三维坐标
|
||
pgnss，GPS数据结构体
|
||
* Output : double，轨迹角
|
||
**************************************************************************/
|
||
double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss);
|
||
|
||
|
||
#endif
|
||
|
||
#pragma once
|