logzhan
/
PDR
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
PDR/1.Software/PDR 1.04/include/pdr_base.h

344 lines
15 KiB
C
Raw Normal View History

优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
/******************** (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 <EFBFBD>㷨<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͷ<EFBFBD>ļ<EFBFBD>
********************************************************************************/
#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 // <20>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>켣ƽ<ECBCA3><C6BD>
/* Macro Definition ----------------------------------------------------------*/
#define PCA_NUM 200
#define ACCURACY_ERR_MAX 1000 // GPS<50><53>accuracy<63><79><EFBFBD><EFBFBD>ֵ,һ<><D2BB><EFBFBD><EFBFBD><EFBFBD>ڳ<EFBFBD>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
#define N 4 // <20><><EFBFBD><EFBFBD>ά<EFBFBD><CEAC>
#define OPEN_SKY 1
#define MAX_NO_GPS_PREDICT 10 // <20><>GPS<50><53>Ϣ״̬<D7B4><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define UN_UPDATE 0 // <20>Ǹ<EFBFBD><C7B8><EFBFBD>״̬
#define DATA_UPDATE 1 // <20><><EFBFBD>ݸ<EFBFBD><DDB8><EFBFBD>
#define ON 1
#define OFF 0
#define PDR_TRUE 1
#define PDR_FALSE 0
#define TYPE_FIX_NONE 0 // PDR<44><52><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define TYPE_FIX_GPS 2 // PDR<44><52><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>ԭʼGPS
#define TYPE_FIX_PDR 1 // PDR<44><52><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ΪPDR<44>ں<EFBFBD>
// PDRЯ<52><D0AF><EFBFBD><EFBFBD>ʽ
#define UNKNOWN 0
#define FORWARD_UP_RIGHT 1 // <20>۰<EFBFBD><DBB0><EFBFBD><EFBFBD>ұۣ<D2B1>ͷ<EFBFBD><CDB7><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⣻or<6F><72><EFBFBD>ۣ<EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define BACKWARD_UP_LEFT 2 // <20>۰<EFBFBD><DBB0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۣ<EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⣻or<6F>ұۣ<D2B1>ͷ<EFBFBD><CDB7><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define BACKWARD_DOWN_RIGHT 3 // <20>۰<EFBFBD><DBB0><EFBFBD><EFBFBD>ұۣ<D2B1>ͷ<EFBFBD><CDB7><EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⣻or<6F><72><EFBFBD>ۣ<EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define FORWARD_DOWN_LEFT 4 // <20>۰<EFBFBD><DBB0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۣ<EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⣻or<6F>ұۣ<D2B1>ͷ<EFBFBD><CDB7><EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define LEFT_UP_FORWARD 5 // <20>㶵<EFBFBD><E3B6B5>ͷ<EFBFBD><CDB7><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD>ǰ
#define RIGHT_UP_BACKWARD 6 // <20>㶵<EFBFBD><E3B6B5>ͷ<EFBFBD><CDB7><EFBFBD>ϣ<EFBFBD><CFA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define RIGHT_DOWN_FORWARD 7 // <20>㶵<EFBFBD><E3B6B5>ͷ<EFBFBD><CDB7><EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD>ǰ
#define LEFT_DOWN_BACKWARD 8 // <20>㶵<EFBFBD><E3B6B5>ͷ<EFBFBD><CDB7><EFBFBD>£<EFBFBD><C2A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// PDR<44><52><EFBFBD><EFBFBD>״̬
#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)
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>س<EFBFBD><D8B3><EFBFBD>
#define WGS84_RE 6378137 // WGS-84<38><34><EFBFBD><EFBFBD><EFBFBD>峤<EFBFBD><E5B3A4><EFBFBD><EFBFBD>
#define WGS84_ECCENTR2 6.69437999014e-3 // <20><>һƫ<D2BB><C6AB><EFBFBD>ʵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>e^2(WGS-84)
#define WGS84_OMEGA_E_DOT 7.2921151467e-5 // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>ٶ<EFBFBD> rad/sec
#define WGS84_GRAVITY 9.7803267714 // <20><><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD> 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ԭʼ<D4AD>ٶ<EFBFBD>תm/s<><73><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>
#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 // <20><><EFBFBD><EFBFBD>256 / SAMPLE_RATE<54><45><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define OPEN_AREA 1 // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(GPS<50>źŽϺ<C5BD>)
#define UN_OPEN_AREA 0 // <20>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(GPS<50>źŽ<C5BA><C5BD><EFBFBD>)
#define DETECTOR_RUN_FREQ 1280 // <20>û<EFBFBD><C3BB>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ms
#define DETECTOR_TYPE_STATIC 0 // <20>û<EFBFBD><C3BB><EFBFBD>ֹ
#define DETECTOR_TYPE_IRREGULAR 1 // <20>޹<EFBFBD><DEB9><EFBFBD><EFBFBD>˶<EFBFBD>
#define DETECTOR_TYPE_HANDHELD 2 // <20>ֳ<EFBFBD><D6B3>˶<EFBFBD>
#define DETECTOR_TYPE_SWINGING 3 // <20><><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>
#define ulong unsigned long
#define uchar unsigned char
typedef enum {
IS_INITIAL = 0,
IS_NORMAL = 1,
}pdrStatus;
typedef struct {
优化卡尔曼滤波器的命名
2022-10-10 23:41:17 +08:00
double Xk[N]; // ״̬<D7B4><CCAC><EFBFBD><EFBFBD>Xk xk[0]: <20><><EFBFBD><EFBFBD>x xk[1]<5D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>y xk[2]<5D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> xk[3] <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double pXk[N]; // <20><><EFBFBD><EFBFBD>Ԥ<EFBFBD><D4A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Xk_predict xk[0]: <20><><EFBFBD><EFBFBD>x xk[1]<5D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>y xk[2]<5D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> xk[3] <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double Zk[N];
double pPk[N][N];
double Pk[N][N];
double Phi[N][N];
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
double hk[N][N];
优化卡尔曼滤波器的命名
2022-10-10 23:41:17 +08:00
double Q[N][N]; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><CBB2><EFBFBD>Q<EFBFBD><51><EFBFBD><EFBFBD>(<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
double R[N][N]; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>R<EFBFBD><52><EFBFBD><EFBFBD>(<28>۲<EFBFBD><DBB2><EFBFBD><EFBFBD><EFBFBD>)
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
double Kk[N][N];
优化卡尔曼滤波器的命名
2022-10-10 23:41:17 +08:00
double Lambda;
double pLat;
double pLon;
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
double initHeading;
}EKFPara_t;
typedef struct{
int fnum;
int deltaStep;
float fsum;
float meanTime;
double lastTime;
}StepPredict;
typedef struct PDR {
uint32_t status; // PDR<44><52>ǰ״̬
uint32_t motionType; // <20>û<EFBFBD><C3BB>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>
uint32_t NoGnssCount; // û<><C3BB>GNSS<53><53>Ϣ<EFBFBD><CFA2><EFBFBD><EFBFBD>
pdrStatus sysStatus; // PDRϵͳ״̬
int sceneType; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(<28>źŽ<C5BA><C5BD><EFBFBD>)
int fusionPdrFlg; // <20>ں<EFBFBD>PDRλ<52>ñ<EFBFBD>־λ, <20><>flgΪ0ʱ<30><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPS
double pllaInit[3]; // <20><>ʼplla<6C><61><EFBFBD><EFBFBD>
double ts; // ʱ<><CAB1><EFBFBD><EFBFBD>
double x0; // <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double y0; // <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double heading; // PDR<44><52><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double lastHeading; // <20><>һ<EFBFBD><D2BB>PDR<44><52><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double deltaHeading; // ƫ<><C6AB><EFBFBD>DZ仯<C7B1><E4BBAF>
double insHeadingOffset; // <20>ߵ<EFBFBD><DFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB>
double imuDeltaHeading; // û<><C3BB><EFBFBD><EFBFBD>һ<EFBFBD><D2BB>GPS<50>źţ<C5BA>PDR<44>仯<EFBFBD>ĽǶȣ<C7B6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
double gpsHeading; // GPS<50><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double lastGpsHeading; // <20><>һ<EFBFBD><D2BB>GPS<50><53><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
double trackHeading; // GPS<50>켣<EFBFBD><ECBCA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>
double gpsSpeed; // GPS<50>ٶ<EFBFBD>
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ulong steps; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ
ulong lastSteps; // <20><>һ<EFBFBD>εIJ<CEB5><C4B2><EFBFBD>
ulong lastGpsSteps; // <20><>һ<EFBFBD><D2BB>GPS<50><53><EFBFBD><EFBFBD>
ulong deltaStepsPerGps; // <20><><EFBFBD><EFBFBD>GPS<50><53><EFBFBD><EFBFBD>֮<EFBFBD>䣬<EFBFBD><E4A3AC><EFBFBD><EFBFBD><EFBFBD>ı仯<C4B1><E4BBAF>
float motionFreq; // <20>˶<EFBFBD>Ƶ<EFBFBD><C6B5>
double gyroTime; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1>
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_t;
typedef struct {
int type;
float accBuffer[PATTERN_RECOGNITION_LEN];
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
}Classifer_t;
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
// <20>û<EFBFBD><C3BB>˶<EFBFBD><CBB6><EFBFBD><EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD><EFBFBD><EFBFBD>
typedef struct DETECTOR {
uint32_t type; // <20>û<EFBFBD><C3BB>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD> <20><> 0:<3A><>ֹ<EFBFBD>˶<EFBFBD> 1<><31><EFBFBD>޹<EFBFBD><DEB9><EFBFBD><EFBFBD>˶<EFBFBD> 2<><32><EFBFBD>ֳ<EFBFBD><D6B3>˶<EFBFBD> 3<><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD>
uint32_t lastType;
uint64_t tick; // <20><><EFBFBD><EFBFBD>ͳ<EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
}Detector_t;
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
Detector_t *GetDetectorObj(void);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
/* Function Declaration ------------------------------------------------------*/
PDR_t* Base_Init(void);
/**----------------------------------------------------------------------
* Function : pdr_resetData
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>PDR<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* Date : 2020/7/21
*
*---------------------------------------------------------------------**/
void pdr_resetData(void);
/**----------------------------------------------------------------------
* Function : pdr_computePCA
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>RESET_PHASE_1\STABLE<EFBFBD>׶<EFBFBD>֮<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PCA<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* Date : 2020/7/21 logzhan
*---------------------------------------------------------------------**/
void ComputePCA(AHRS_t* ahrs);
/**---------------------------------------------------------------------
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
* Function : DetectorUpdate
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֻ<EFBFBD>Я<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<EFBFBD><EFBFBD>
* Date : 2022/10/15 logzhan
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
*---------------------------------------------------------------------**/
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
void DetectorUpdate(Detector_t* detector);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
void gpsUpdateCb(GNSS_t* gps);
/**************************************************************************
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD>
* Input : stepPredict<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԥ<EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><EFBFBD>
timestamp<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
steps_last<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>β<EFBFBD><EFBFBD><EFBFBD>
steps<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>β<EFBFBD><EFBFBD><EFBFBD>
**************************************************************************/
void calStepLastTime(StepPredict *stepPredict, double timestamp, unsigned long steps_last, unsigned long steps);
/**************************************************************************
* Description : Ԥ<EFBFBD>ⲽ<EFBFBD><EFBFBD>
* Input : stepPredict<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԥ<EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><EFBFBD>
timestamp<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
steps_last<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>β<EFBFBD><EFBFBD><EFBFBD>
gnssVel<EFBFBD><EFBFBD>gps<EFBFBD>ٶ<EFBFBD>
* Output : int<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD>
**************************************************************************/
int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_last, float gnssVel);
/**----------------------------------------------------------------------
* Function : stateRecognition
* Description : <EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD><EFBFBD>ٶȼ<EFBFBD><EFBFBD>жϵ<EFBFBD>ǰ״̬<EFBFBD>Ƿ<EFBFBD>ƽ<EFBFBD><EFBFBD> 1: <EFBFBD><EFBFBD>ƽ<EFBFBD><EFBFBD> 0: ƽ<EFBFBD><EFBFBD>
* Date : 2020/7/22 logzhan
*---------------------------------------------------------------------**/
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
void StateRecognition(float *acc, Classifer_t *sys);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
/**************************************************************************
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPS<EFBFBD>Ƕȡ<EFBFBD>GPS<EFBFBD>켣<EFBFBD>ǡ<EFBFBD>PDR<EFBFBD>Ƕȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><EFBFBD><EFBFBD>
* Input : g_pdr<EFBFBD><EFBFBD>PDR<EFBFBD>ṹ<EFBFBD><EFBFBD>
sys<EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
gps_yaw<EFBFBD><EFBFBD>GPS<EFBFBD>켣<EFBFBD><EFBFBD>
G_yaw<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ss_data<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
* Output : double<EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><EFBFBD><EFBFBD>
**************************************************************************/
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
double calPredAngle(PDR_t *g_pdr, Classifer_t *sys, double gps_yaw, double G_yaw, IMU_t *ss_data);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
/**************************************************************************
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPS<EFBFBD>Ƕȡ<EFBFBD>GPS<EFBFBD>켣<EFBFBD>ǡ<EFBFBD>PDR<EFBFBD>Ƕȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><EFBFBD><EFBFBD>
* Input : g_pdr<EFBFBD><EFBFBD>PDR<EFBFBD>ṹ<EFBFBD><EFBFBD>
steps<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>β<EFBFBD><EFBFBD><EFBFBD>
steps_last<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>β<EFBFBD><EFBFBD><EFBFBD>
stepPredict<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԥ<EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><EFBFBD>
pdr_angle<EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD>߷<EFBFBD><EFBFBD><EFBFBD>
scene_type<EFBFBD><EFBFBD>GPS<EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͷǿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
ss_data<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
pgnss<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
kf<EFBFBD><EFBFBD>EKF<EFBFBD>ṹ<EFBFBD><EFBFBD>
pdr, PDRԤ<EFBFBD><EFBFBD>λ<EFBFBD>óɹ<EFBFBD><EFBFBD><EFBFBD>־λ
**************************************************************************/
void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict,IMU_t *ss_data,
GNSS_t *pgnss, EKFPara_t *kf);
/**----------------------------------------------------------------------
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
* Function : DetUserStatic
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ھ<EFBFBD>ֹ״̬
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
* Date : 2022/10/15 logzhan
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
*---------------------------------------------------------------------**/
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
int DetUserStatic(PDR_t *g_pdr, GNSS_t *pgnss, unsigned long delSteps);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
/**----------------------------------------------------------------------
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
* Function : OutputRawGnssPos
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPSλ<EFBFBD><EFBFBD>
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
* Date : 2022/10/15 logzhan
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
*---------------------------------------------------------------------**/
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
void OutputRawGnssPos(GNSS_t *pgnss, LctFs_t *result);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
/**----------------------------------------------------------------------
* Function : resetOutputResult
* Description : 1<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼλ<EFBFBD>á<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* 2<EFBFBD><EFBFBD>GPSֵ<EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>result
* pgnss<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
* plla_init<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>NED<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵԭ<EFBFBD>㾭γ<EFBFBD><EFBFBD>
* x_init<EFBFBD><EFBFBD>y_init<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>NED<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵԭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>n<EFBFBD><EFBFBD>e<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* kf<EFBFBD><EFBFBD> EKF<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
* Date : 2020/07/08 logzhan
*---------------------------------------------------------------------**/
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
int ResetOutputResult(GNSS_t *pgnss, PDR_t* g_pdr, EKFPara_t *kf, LctFs_t *result);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
/**----------------------------------------------------------------------
* Function : detIsCarMode
* Description : ʶ<EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><EFBFBD>dz<EFBFBD><EFBFBD><EFBFBD>ģʽ
* Input : pgnss<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
g_pdr<EFBFBD><EFBFBD>PDR<EFBFBD>ṹ<EFBFBD><EFBFBD>
delSteps<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ȳ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ı仯<EFBFBD><EFBFBD>
time<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* Output : int<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ<EFBFBD><EFBFBD>־λ
* Date : 2021/01/28 logzhan
*---------------------------------------------------------------------**/
int DetectCarMode(GNSS_t *pgnss, PDR_t *g_pdr, unsigned long delSteps, int *time);
/**----------------------------------------------------------------------
* Function : detPdrToReset
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>PDRϵͳ<EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PDR<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD>󣬻<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>PDR<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD>λ<EFBFBD>á<EFBFBD>
* Date : 2021/01/28 logzhan
*---------------------------------------------------------------------**/
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
int DetectPdrToReset(double pdr_angle, int *gpscnt, unsigned long deltsteps, PDR_t *g_pdr);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
/**************************************************************************
* Description : <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>EKF<EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD>
* Input : pgnss<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
plla_init<EFBFBD><EFBFBD>NED<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵԭ<EFBFBD>㾭γ<EFBFBD><EFBFBD>
kf<EFBFBD><EFBFBD> EKF<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
x_init<EFBFBD><EFBFBD>y_init<EFBFBD><EFBFBD>NED<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵԭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>n<EFBFBD><EFBFBD>e<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
sys_status<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λϵͳ״̬<EFBFBD><EFBFBD>δ<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѳ<EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* Output : int<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>־λ
result<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ṹ<EFBFBD><EFBFBD>
**************************************************************************/
重构部分函数命名和结构体，运行结果不变
2022-10-15 16:07:19 +08:00
int InitProc(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr, LctFs_t *result);
优化卡尔曼矩阵计算部分
2022-10-07 23:49:43 +08:00
/**************************************************************************
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>EKF<EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Q<EFBFBD><EFBFBD>R
* Input : scane_type, <EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͷǿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
nmea_data<EFBFBD><EFBFBD>NMEA<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
g_pdr<EFBFBD><EFBFBD>PDR<EFBFBD>ṹ<EFBFBD><EFBFBD>
sys<EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
kf<EFBFBD><EFBFBD>EKF<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
**************************************************************************/
/**************************************************************************
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPS<EFBFBD>켣<EFBFBD><EFBFBD>
* Input : gpsPos<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD>ʷ<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ECEF<EFBFBD>µ<EFBFBD><EFBFBD><EFBFBD>ά<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
pgnss<EFBFBD><EFBFBD>GPS<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
* Output : double<EFBFBD><EFBFBD><EFBFBD>켣<EFBFBD><EFBFBD>
**************************************************************************/
double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss);
#endif
#pragma once