From 4028c676c4e2c2e83a618486999c48a53936b6a2 Mon Sep 17 00:00:00 2001 From: logzhan <719901725@qq.com> Date: Sun, 16 Oct 2022 23:21:37 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=E9=83=A8=E5=88=86=E5=87=BD?= =?UTF-8?q?=E6=95=B0=E5=91=BD=E5=90=8D=EF=BC=8C=E5=8E=BB=E9=99=A4PCA?= =?UTF-8?q?=E8=AE=A1=E7=AE=97=E6=96=B9=E5=90=91=E9=83=A8=E5=88=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- 1.Software/PDR 1.04/include/Kalman.h | 8 - 1.Software/PDR 1.04/include/Location.h | 44 +- 1.Software/PDR 1.04/include/pdr_api.h | 13 +- 1.Software/PDR 1.04/include/pdr_base.h | 39 +- 1.Software/PDR 1.04/src/Detector.c | 59 ++- 1.Software/PDR 1.04/src/Interface.c | 88 ++-- 1.Software/PDR 1.04/src/Kalman.c | 143 +------ 1.Software/PDR 1.04/src/Location.c | 208 ++++++++-- 1.Software/PDR 1.04/src/Main.cpp | 11 +- 1.Software/PDR 1.04/src/Munkres.c | 40 +- 1.Software/PDR 1.04/src/PDRBase.c | 545 +++---------------------- 11 files changed, 347 insertions(+), 851 deletions(-) diff --git a/1.Software/PDR 1.04/include/Kalman.h b/1.Software/PDR 1.04/include/Kalman.h index 2329ba4..3333183 100644 --- a/1.Software/PDR 1.04/include/Kalman.h +++ b/1.Software/PDR 1.04/include/Kalman.h @@ -47,14 +47,6 @@ void EKFCalQRMatrix(Nmea_t* nmea_data, PDR_t* g_pdr, Classifer_t* sys, EKFPara_t void EKFStateUpdate(EKFPara_t* kf, GNSS_t* pgnss, Classifer_t* sys, PDR_t* g_pdr, int scene_type); -/**---------------------------------------------------------------------- -* Function : pdr_gnssInsLocFusion -* Description : PDR的GNSS和INS融合定位 -* Date : 2020/07/09 logzhan -*---------------------------------------------------------------------**/ -void GnssInsLocFusion(GNSS_t* pgnss, PDR_t* g_pdr, Classifer_t* sys, double yaw_bias, - EKFPara_t* kf, LctFs_t* res); - #ifdef __cplusplus } #endif diff --git a/1.Software/PDR 1.04/include/Location.h b/1.Software/PDR 1.04/include/Location.h index 79e9548..66cbf98 100644 --- a/1.Software/PDR 1.04/include/Location.h +++ b/1.Software/PDR 1.04/include/Location.h @@ -17,7 +17,7 @@ extern "C" { * Description : PDR导航系统初始化 * Date : 2022/9/16 *---------------------------------------------------------------------**/ -void PDRNav_Init(void); +void PDR_Init(void); /**--------------------------------------------------------------------- * Function : InsLocation @@ -86,10 +86,48 @@ int GnssInsFusionLocation(Nmea_t* nmea_data, EKFPara_t* kf, LctFs_t* result); * Description : PDS GNSS定位信息初始化 * Date : 2020/2/1 logzhan *---------------------------------------------------------------------**/ -void InitGnssInfo(void); +void InitGnssInfo(void); -void GnssUpdate(GNSS_t* gps, Nmea_t* nmea); +/**---------------------------------------------------------------------- +* Function : GnssUpdate +* Description : nmea数据结构转gnss数据 +* Date : 2020/07/08 logzhan +* 2020/02/09 logzhan : 函数对应原pdr_gpsUpdate函数， +* 经过引用分析发现每次GPS更新拷贝gnss结构体和nmea结构体其实没 +* 什么用，还增加运算量 +*---------------------------------------------------------------------**/ +void GnssUpdate(GNSS_t* gps, Nmea_t* nmea); +/**---------------------------------------------------------------------- +* Function : OutputRawGnssPos +* Description : 输出GPS位置 +* Date : 2022/10/15 logzhan +*---------------------------------------------------------------------**/ +void OutputRawGnssPos(GNSS_t* pgnss, LctFs_t* result); + +/**---------------------------------------------------------------------- +* Function : ResetOutputLoction +* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数 +* 2、GPS值赋值给result +* Date : 2022/09/19 logzhan +*---------------------------------------------------------------------**/ +int ResetOutputLoction(GNSS_t* pgnss, PDR_t* g_pdr, EKFPara_t* kf, LctFs_t* result); + + +/**---------------------------------------------------------------------- +* Function : EkfGnssInsLocFusion +* Description : 采用EKF对GNSS和INS融合定位 +* Date : 2022/09/21 logzhan +*---------------------------------------------------------------------**/ +void EkfGnssInsLocFusion(GNSS_t* pgnss, PDR_t* g_pdr, Classifer_t* sys, double yaw_bias, + EKFPara_t* kf, LctFs_t* res); + +/**---------------------------------------------------------------------- +* Function : PDRInitialAlignment +* Description : PDR初始对准 +* Date : 2022/09/19 logzhan +*---------------------------------------------------------------------**/ +int GnssInsInitialAlignment(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, LctFs_t* result); #ifdef __cplusplus } diff --git a/1.Software/PDR 1.04/include/pdr_api.h b/1.Software/PDR 1.04/include/pdr_api.h index 4052deb..5967c30 100644 --- a/1.Software/PDR 1.04/include/pdr_api.h +++ b/1.Software/PDR 1.04/include/pdr_api.h @@ -44,17 +44,12 @@ typedef struct _PosFusion{ void Algorithm_Init(void); /**--------------------------------------------------------------------- -* Function : LocationMainLoop +* Function : PDRLocationMainLoop * Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置 * 迹平滑窗口的初始化 -* Input : ss_data， 传感器数据 - nmea_data, NMEA数据 - result，定位结果 -* Output : int, 输出标志位 -* Date : 2020/7/3 logzhan +* Date : 2022/10/16 logzhan *---------------------------------------------------------------------**/ -int LocationMainLoop(IMU_t* ImuData, Nmea_t* NmeaData, LctFs_t* LocFusion, - FILE *fp_gps); +int PDRLocationMainLoop(IMU_t* ImuData, Nmea_t* NmeaData, LctFs_t* LocFusion); /**--------------------------------------------------------------------- * Function : pdr_locationMainLoopStr @@ -63,7 +58,7 @@ int LocationMainLoop(IMU_t* ImuData, Nmea_t* NmeaData, LctFs_t* LocFusion, * Date : 2020/07/03 logzhan * 2020/02/02 logzhan : 增加宏控制是否开启输出平滑 *---------------------------------------------------------------------**/ -int ParseLineAndUpdate(char* line, LctFs_t* result); +int ParseDataAndUpdate(char* line, LctFs_t* result); /**--------------------------------------------------------------------- * Function : pdr_initRmc diff --git a/1.Software/PDR 1.04/include/pdr_base.h b/1.Software/PDR 1.04/include/pdr_base.h index 3c34da8..40fde51 100644 --- a/1.Software/PDR 1.04/include/pdr_base.h +++ b/1.Software/PDR 1.04/include/pdr_base.h @@ -128,7 +128,7 @@ typedef struct{ typedef struct PDR { uint32_t status; // PDR当前状态 - uint32_t motionType; // 用户运动类型 + uint32_t MotionType; // 用户运动类型 uint32_t NoGnssCount; // 没有GNSS信息次数 pdrStatus sysStatus; // PDR系统状态 int sceneType; // 场景类型：1：开阔区域，0：非开阔区域(信号较弱) @@ -189,7 +189,7 @@ PDR_t* Base_Init(void); * Date : 2020/7/21 * *---------------------------------------------------------------------**/ -void pdr_resetData(void); +void ResetPDRData(void); /**---------------------------------------------------------------------- * Function : pdr_computePCA @@ -206,8 +206,6 @@ void ComputePCA(AHRS_t* ahrs); void DetectorUpdate(Detector_t* detector); -void gpsUpdateCb(GNSS_t* gps); - /************************************************************************** * Description : 计算每步所耗时间 * Input : stepPredict， 步数预测结构体 @@ -234,16 +232,6 @@ int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_ *---------------------------------------------------------------------**/ void StateRecognition(float *acc, Classifer_t *sys); -/************************************************************************** -* Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向 -* Input : g_pdr，PDR结构体 - sys，运动幅度数据结构体 - gps_yaw，GPS轨迹角 - G_yaw，GPS方向角 - ss_data， 传感器数据结构体 -* Output : double，用户行走方向 -**************************************************************************/ -double calPredAngle(PDR_t *g_pdr, Classifer_t *sys, double gps_yaw, double G_yaw, IMU_t *ss_data); /************************************************************************** * Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向 @@ -258,7 +246,7 @@ double calPredAngle(PDR_t *g_pdr, Classifer_t *sys, double gps_yaw, double G_yaw kf，EKF结构体 pdr, PDR预测位置成功标志位 **************************************************************************/ -void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict,IMU_t *ss_data, +void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict,IMU_t *ss_data, GNSS_t *pgnss, EKFPara_t *kf); /**---------------------------------------------------------------------- @@ -268,25 +256,6 @@ void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict,IMU_t *ss_data, *---------------------------------------------------------------------**/ int DetUserStatic(PDR_t *g_pdr, GNSS_t *pgnss, unsigned long delSteps); -/**---------------------------------------------------------------------- -* Function : OutputRawGnssPos -* Description : 输出GPS位置 -* Date : 2022/10/15 logzhan -*---------------------------------------------------------------------**/ -void OutputRawGnssPos(GNSS_t *pgnss, LctFs_t *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_t* g_pdr, EKFPara_t *kf, LctFs_t *result); - /**---------------------------------------------------------------------- * Function : detIsCarMode * Description : 识别是否是车载模式 @@ -317,7 +286,7 @@ int DetectPdrToReset(double pdr_angle, int *gpscnt, unsigned long deltsteps, PDR * Output : int，初始化标志位 result，定位结果结构体 **************************************************************************/ -int InitProc(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr, LctFs_t *result); +int GnssInsInitialAlignment(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr, LctFs_t *result); /************************************************************************** * Description : 设置EKF滤波器的Q和R diff --git a/1.Software/PDR 1.04/src/Detector.c b/1.Software/PDR 1.04/src/Detector.c index 65bffe6..2c7ceb8 100644 --- a/1.Software/PDR 1.04/src/Detector.c +++ b/1.Software/PDR 1.04/src/Detector.c @@ -34,17 +34,12 @@ #define INC_PTR(buf, ptr) (((ptr) + 1) % ((buf)->length + 1)) #define DEC_PTR(buf, ptr) (((ptr) + (buf)->length) % ((buf)->length + 1)) #define COST(i, j) cost[(i) + (j) * number] -#define CALIBRATE_ON 1 -#define CALIBRATE_OFF 0 -extern int debugCount; -static float ellipsoidPoint[3]; -static float ellipsoidScale[3]; /* Global Variable Definition ------------------------------------------------*/ -static Detector_t g_detector; +static Detector_t pDetector; static BUFFER_LONG g_mol_buf[2]; -static Filter_t g_mol_flt[2]; +static Filter_t g_mol_flt[2]; const double g_mol_flt_b[] = { 4.96135120696701e-05, 0.000496135120696701, 0.00223260804313515, 0.00595362144836041, 0.0104188375346307, 0.0125026050415569, 0.0104188375346307, 0.00595362144836041, 0.00223260804313515, @@ -59,10 +54,7 @@ static BUFFER_SHORT g_acc_tmp_buf[3]; BUFFER_SHORT g_gyr_amp_buf[3]; static BUFFER_SHORT g_gyr_tmp_buf[3]; -static float mag_buff[MAG_BUF_LEN][3]; -static int mag_count; -static unsigned char mag_calibrate; -//static double mag_time; + static int g_label[5] = { 0 }; /* Function Definition -------------------------------------------------------------------------- */ @@ -73,7 +65,7 @@ static int g_label[5] = { 0 }; * Date : 2020/02/16 logzhan *---------------------------------------------------------------------**/ Detector_t* GetDetectorObj(void) { - return &g_detector; + return &pDetector; } /**--------------------------------------------------------------------- @@ -83,7 +75,7 @@ Detector_t* GetDetectorObj(void) { *---------------------------------------------------------------------**/ Detector_t* Detector_Init(void) { DetectorReset(); - return &g_detector; + return &pDetector; } /**--------------------------------------------------------------------- @@ -93,40 +85,39 @@ Detector_t* Detector_Init(void) { *---------------------------------------------------------------------**/ void DetectorReset(void) { - g_detector.tick = 0; - g_detector.lastType = DETECTOR_TYPE_STATIC; + pDetector.tick = 0; + pDetector.lastType = DETECTOR_TYPE_STATIC; BufferInit((Buffer_t *)&g_mol_buf[ACCE], "acce_mold_filtered", BUFFER_TYPE_QUEUE, FLT_BUF_LEN); BufferInit((Buffer_t *)&g_mol_buf[GYRO], "gyro_mold_filtered", BUFFER_TYPE_QUEUE, FLT_BUF_LEN); + BufferInit((Buffer_t *)&g_acc_frq_buf[0], "acc_frq_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); BufferInit((Buffer_t *)&g_acc_frq_buf[1], "acc_frq_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); BufferInit((Buffer_t *)&g_acc_frq_buf[2], "acc_frq_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + BufferInit((Buffer_t *)&g_acc_amp_buf[0], "acc_amp_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); BufferInit((Buffer_t *)&g_acc_amp_buf[1], "acc_amp_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); BufferInit((Buffer_t *)&g_acc_amp_buf[2], "acc_amp_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + BufferInit((Buffer_t *)&g_acc_tmp_buf[0], "acc_tmp_buf_0", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); BufferInit((Buffer_t *)&g_acc_tmp_buf[1], "acc_tmp_buf_1", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); BufferInit((Buffer_t *)&g_acc_tmp_buf[2], "acc_tmp_buf_2", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); + BufferInit((Buffer_t *)&g_gyr_frq_buf[0], "gyr_frq_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); BufferInit((Buffer_t *)&g_gyr_frq_buf[1], "gyr_frq_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); BufferInit((Buffer_t *)&g_gyr_frq_buf[2], "gyr_frq_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + BufferInit((Buffer_t *)&g_gyr_amp_buf[0], "gyr_amp_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); BufferInit((Buffer_t *)&g_gyr_amp_buf[1], "gyr_amp_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); BufferInit((Buffer_t *)&g_gyr_amp_buf[2], "gyr_amp_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN); + BufferInit((Buffer_t *)&g_gyr_tmp_buf[0], "gyr_tmp_buf_0", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); BufferInit((Buffer_t *)&g_gyr_tmp_buf[1], "gyr_tmp_buf_1", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); BufferInit((Buffer_t *)&g_gyr_tmp_buf[2], "gyr_tmp_buf_2", BUFFER_TYPE_QUEUE, TMP_BUF_LEN); + FilterSetCoef(&g_mol_flt[ACCE], ARR_LEN(g_mol_flt_b), (double *)g_mol_flt_b, (double *)g_mol_flt_a, (double)g_mol_flt_h0); FilterSetCoef(&g_mol_flt[GYRO], ARR_LEN(g_mol_flt_b), (double *)g_mol_flt_b, (double *)g_mol_flt_a, (double)g_mol_flt_h0); - memset(*mag_buff,0,sizeof(mag_buff)); - mag_count = 0; - mag_calibrate = CALIBRATE_OFF; - ellipsoidPoint[0] = 0; - ellipsoidPoint[1] = 0; - ellipsoidPoint[2] = 0; - ellipsoidScale[0] = 1; - ellipsoidScale[1] = 1; - ellipsoidScale[2] = 1; + memset((void *)g_label, 0, sizeof(g_label)); } @@ -144,16 +135,16 @@ void DetectorUpdateIMU(IMU_t* imu) { BufferPush((Buffer_t *)&g_mol_buf[GYRO], (float)FilterFilter(&g_mol_flt[GYRO], GET_MOL(imu->gyr.s))); // g_tick_d 实际上是1.28s检测一次，不然检测的频率过高 - if ((g_detector.tick % (int)(1.28 * IMU_SAMPLING_FREQUENCY)) == 0) { + if ((pDetector.tick % (int)(1.28 * IMU_SAMPLING_FREQUENCY)) == 0) { // 检测用户的运动类型 int detectResult = DetectMotionType(); - if (detectResult == DETECTOR_NO_ERROR && g_detector.lastType != g_detector.type) { - DetectorUpdate(&g_detector); - g_detector.lastType = g_detector.type; + if (detectResult == DETECTOR_NO_ERROR && pDetector.lastType != pDetector.type) { + DetectorUpdate(&pDetector); + pDetector.lastType = pDetector.type; } } - ++g_detector.tick; + ++pDetector.tick; } /**--------------------------------------------------------------------- @@ -654,8 +645,8 @@ int DetectMotionType() { if (DETECTOR_NO_ERROR != (ret = fft_buffer(s_fft, (Buffer_t *)&g_mol_buf[GYRO]))) { return ret; } - if (DETECTOR_NO_ERROR != (ret = find_peaks(frq, amp, 3, s_fft, 1024, 5, (DETECTOR_TYPE_SWINGING == g_detector.type || - DETECTOR_TYPE_HANDHELD == g_detector.type) ? 20.0f : 100.0f))) { + if (DETECTOR_NO_ERROR != (ret = find_peaks(frq, amp, 3, s_fft, 1024, 5, (DETECTOR_TYPE_SWINGING == pDetector.type || + DETECTOR_TYPE_HANDHELD == pDetector.type) ? 20.0f : 100.0f))) { return ret; } if (DETECTOR_NO_ERROR != (ret = track_frequency(g_gyr_frq_buf, g_gyr_amp_buf, g_gyr_tmp_buf, frq, amp, 3))) { @@ -664,14 +655,14 @@ int DetectMotionType() { get_feature(s_feature, 5); - g_detector.type = pdr_detectorPredict(s_feature); + pDetector.type = pdr_detectorPredict(s_feature); /* debounce */ for (i = 0; i < ARR_LEN(g_label) - 1; ++i) { g_label[i] = g_label[i + 1]; } - g_label[i] = g_detector.type; - g_detector.type = debounce(g_label, ARR_LEN(g_label)); + g_label[i] = pDetector.type; + pDetector.type = debounce(g_label, ARR_LEN(g_label)); return DETECTOR_NO_ERROR; } diff --git a/1.Software/PDR 1.04/src/Interface.c b/1.Software/PDR 1.04/src/Interface.c index ba7e97f..44e068e 100644 --- a/1.Software/PDR 1.04/src/Interface.c +++ b/1.Software/PDR 1.04/src/Interface.c @@ -20,8 +20,8 @@ const char* PDR_Version = "2.0"; Sensor_t SensorDataHist[IMU_LAST_COUNT] = {{0}}; LatLng_t llaLast = { 0 }; -Nmea_t NmeaData; -IMU_t ImuData; +Nmea_t Nmea; +IMU_t Imu; /* Extern Variable Definition ------------------------------------------------*/ extern EKFPara_t g_kfPara; @@ -99,109 +99,87 @@ void ClearNmeaFlg(Nmea_t * ln){ *---------------------------------------------------------------------**/ void Algorithm_Init(void) { // 导航相关初始化 - PDRNav_Init(); + PDR_Init(); // 计步器初始化 Pedometer_Init(); // 初始化sensor历史缓存 memset(SensorDataHist, 0, sizeof(SensorDataHist)); - memset(&NmeaData, 0, sizeof(NmeaData)); - memset(&ImuData, 0, sizeof(ImuData)); + memset(&Nmea, 0, sizeof(Nmea)); + memset(&Imu, 0, sizeof(Imu)); } /**--------------------------------------------------------------------- -* Function : ParseLineAndUpdate +* Function : ParseDataAndUpdate * Description : 解析字符串信息并更新 -* Date : 2020/07/03 logzhan -* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑 +* Date : 2022/10/16 logzhan *---------------------------------------------------------------------**/ -int ParseLineAndUpdate(char* line, LctFs_t* locFusion) { - - ParseLineStr(line, &ImuData, &NmeaData); - return LocationMainLoop(&ImuData, &NmeaData, locFusion, NULL); +int ParseDataAndUpdate(char* line, LctFs_t* LocFusion) +{ + ParseLineStr(line, &Imu, &Nmea); + // 给定Imu和Nmea结构体，返回融合位置 + return PDRLocationMainLoop(&Imu, &Nmea, LocFusion); } /**--------------------------------------------------------------------- -* Function : LocationMainLoop +* Function : PDRLocationMainLoop * Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置 * 迹平滑窗口的初始化 * Date : 2020/07/03 logzhan * 2020/02/02 logzhan : 增加宏控制是否开启输出平滑 *---------------------------------------------------------------------**/ -int LocationMainLoop(IMU_t *ImuData, Nmea_t *NmeaData, LctFs_t *LocFusion, FILE *fpGps) { +int PDRLocationMainLoop(IMU_t *imu, Nmea_t *nmea, LctFs_t *LocFusion) { int type = 0; - if (ImuData->gyr.update) { + if (imu->gyr.update) { // 如果陀螺仪更新则采用惯性传感器计算 - InsLocation(ImuData,&g_kfPara); - ImuData->gyr.update = UN_UPDATE; + InsLocation(imu,&g_kfPara); + imu->gyr.update = UN_UPDATE; } // 如果GPS不更新，返回 - if (!NmeaData->update)return TYPE_FIX_NONE; + if (!nmea->update)return TYPE_FIX_NONE; // 写GPS相关LOG信息 - SaveGnssInfo(NmeaData, LocFusion, fpGps); + SaveGnssInfo(nmea, LocFusion, NULL); // 有GPS则采用GPS融合定位 - int flag = GnssInsFusionLocation(NmeaData, &g_kfPara, LocFusion); + int flag = GnssInsFusionLocation(nmea, &g_kfPara, LocFusion); if (flag != TYPE_FIX_NONE) { // 如果是开车这种，直接输出GPS，不进行平滑处理 - if (flag == TYPE_FIX_GPS) { - LocFusion->latitude = R2D(LocFusion->latitude); - LocFusion->longitudinal = R2D(LocFusion->longitudinal); + LocFusion->latitude = R2D(LocFusion->latitude); + LocFusion->longitudinal = R2D(LocFusion->longitudinal); - } else { - LatLng_t lla = { 0 }; - lla.lat = R2D(LocFusion->latitude); - lla.lon = R2D(LocFusion->longitudinal); - - if (LocFusion->last_lat != 0.0 && LocFusion->last_lon != 0.0 && - CalDistance(lla, llaLast) > 10) { - } - LocFusion->latitude = R2D(LocFusion->latitude); - LocFusion->longitudinal = R2D(LocFusion->longitudinal); - - LocFusion->latitude = (LocFusion->latitude) * RADIAN_PER_DEG; - LocFusion->longitudinal = (LocFusion->longitudinal) * RADIAN_PER_DEG; - - LocFusion->latitude = R2D(LocFusion->latitude); - LocFusion->longitudinal = R2D(LocFusion->longitudinal); - - LocFusion->last_lat = LocFusion->latitude; - LocFusion->last_lon = LocFusion->longitudinal; - - llaLast.lat = lla.lat; - llaLast.lon = lla.lon; - } + LocFusion->last_lat = LocFusion->latitude; + LocFusion->last_lon = LocFusion->longitudinal; type = 1; }else if (LocFusion->last_lat != 0.0 && LocFusion->last_lon != 0.0) { LocFusion->latitude = LocFusion->last_lat; LocFusion->longitudinal = LocFusion->last_lon; type = 1; } - ClearNmeaFlg(NmeaData); + ClearNmeaFlg(nmea); return type; } /**--------------------------------------------------------------------- -* Function : pdr_saveGnssInfo +* Function : SaveGnssInfo * Description : 保存GPS相关信息 -* Date : 2020/7/3 logzhan +* Date : 2022/10/16 logzhan *---------------------------------------------------------------------**/ -void SaveGnssInfo(Nmea_t* nmea_data, LctFs_t* result, FILE* fp_gps) +void SaveGnssInfo(Nmea_t* nmea, LctFs_t* result, FILE* fp_gps) { - NmeaRMC_t rmc = nmea_data->rmc[1]; + NmeaRMC_t rmc = nmea->rmc[1]; // 选取更新时间的rmc - if (nmea_data->rmc[0].time > nmea_data->rmc[1].time) { - rmc = nmea_data->rmc[0]; + if (nmea->rmc[0].time > nmea->rmc[1].time) { + rmc = nmea->rmc[0]; } if (rmc.update && (rmc.status == POSITIONING_Y)) { char str[256]; memset(str, 0, sizeof(str)); float accuracy = -1.0f; - if (fabs(rmc.time - nmea_data->accuracy.time) < 500){ - accuracy = nmea_data->accuracy.err; + if (fabs(rmc.time - nmea->accuracy.time) < 500){ + accuracy = nmea->accuracy.err; } sprintf(str, "%f,%.10f,%d,%.10f,%d,%.10f,%f,%f", rmc.time, rmc.latitude, rmc.rmc_latitude_ns,rmc.longitudinal, rmc.longitudinal_ew, rmc.cog, accuracy, diff --git a/1.Software/PDR 1.04/src/Kalman.c b/1.Software/PDR 1.04/src/Kalman.c index 49be27e..a28aa00 100644 --- a/1.Software/PDR 1.04/src/Kalman.c +++ b/1.Software/PDR 1.04/src/Kalman.c @@ -27,13 +27,13 @@ static double EkfMatBuf[7][N][N] = { { {0.0} } }; extern double GpsPos[HIST_GPS_NUM][3]; extern int debugCount; -extern double angle_mean[3]; + static int hold_type; static double b_timestamp; static double b_last_time; static double attitude_yaw; -static Detector_t *g_detector; +static Detector_t *pDetector; static int hold_type; static double BUL_duration; static double FUR_duration; @@ -76,7 +76,7 @@ void EKFStatePredict(EKFPara_t *ekf, double stepLen, PDR_t* pdr, int step) { // angle = g_pdr->heading; //} - if (pdr->motionType != PDR_MOTION_TYPE_HANDHELD) { + if (pdr->MotionType != PDR_MOTION_TYPE_HANDHELD) { deltaHeading = 0; } @@ -503,94 +503,6 @@ void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict, IMU_t *ss_data, } } -/**---------------------------------------------------------------------- -* Function : ResetOutputResult -* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数 -* 2、GPS值赋值给result -* Date : 2022/09/19 logzhan -*---------------------------------------------------------------------**/ -int ResetOutputResult(GNSS_t *pgnss, PDR_t* g_pdr, EKFPara_t *kf, LctFs_t *result) { - - double stepLength = 0.7; - double ned[3] = { 0 }; - - g_pdr->pllaInit[0] = pgnss->lat; - g_pdr->pllaInit[1] = pgnss->lon; - g_pdr->pllaInit[2] = 0; - WGS842NED(g_pdr->pllaInit, g_pdr->pllaInit, ned); - - g_pdr->x0 = ned[0]; - g_pdr->y0 = ned[1]; - - kf->pXk[0] = ned[0]; - kf->pXk[1] = ned[1]; - kf->pXk[2] = stepLength; - // 重置输出GPS时，GPS的航向角不一定准 - kf->pXk[3] = pgnss->yaw*RADIAN_PER_DEG; - - for (uchar i = 0; i < N; i++) { - kf->Xk[i] = kf->pXk[i]; - } - // 清除卡尔曼Q矩阵和R矩阵 - for (int i = 0; i < N; i++) { - for (int l = 0; l < N; l++) { - kf->Q[i][l] = 0.0; - kf->R[i][l] = 0.0; - } - } - // 输出GPS位置结果 - OutputRawGnssPos(pgnss, result); - g_pdr->NoGnssCount = 0; - return PDR_TRUE; -} - -int InitProc(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr,LctFs_t *result) -{ - double stepLen = 0.7; // 初始运动步长为0.7m - if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.5 && pgnss->yaw != -1) { - // plla 类似位置坐标的原点(单位是经纬度) - - g_pdr->pllaInit[0] = pgnss->lat; - g_pdr->pllaInit[1] = pgnss->lon; - g_pdr->pllaInit[2] = 0; - - // plla 转 ned - double ned[3] = { 0 }; - WGS842NED(g_pdr->pllaInit, g_pdr->pllaInit, ned); - - // 在NED坐标系的计算, 单位是米 - g_pdr->x0 = ned[0]; - g_pdr->y0 = ned[1]; - - // 初始化卡尔曼滤波器的观测量 - kf->pXk[0] = ned[0]; // 状态量1: 北向x - kf->pXk[1] = ned[1]; // 状态量2：东向y - kf->pXk[2] = stepLen; // 状态量3：步长 - kf->pXk[3] = pgnss->yaw * RADIAN_PER_DEG; // 状态量4：航向角 - - for (uchar i = 0; i < N; i++) { - kf->Xk[i] = kf->pXk[i]; - } - - kf->initHeading = kf->pXk[3]; - for (int i = 0; i < N; i++) { - for (int l = 0; l < N; l++) { - kf->Q[i][l] = 0.0; - kf->R[i][l] = 0.0; - } - } - OutputRawGnssPos(pgnss, result); - g_pdr->sysStatus = IS_NORMAL; - - return PDR_TRUE; - } - - if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel > 0 && pgnss->yaw != -1) { - OutputRawGnssPos(pgnss, result); - return PDR_TRUE; - } - return PDR_FALSE; -} /**---------------------------------------------------------------------- * Function : EKFCalQRMatrix @@ -602,8 +514,8 @@ int InitProc(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr,LctFs_t *result) void EKFCalQRMatrix(Nmea_t *nmea, PDR_t *g_pdr, Classifer_t *sys, EKFPara_t *kf) { - if (g_pdr->motionType != PDR_MOTION_TYPE_HANDHELD && - g_pdr->motionType != PDR_MOTION_TYPE_STATIC) { + if (g_pdr->MotionType != PDR_MOTION_TYPE_HANDHELD && + g_pdr->MotionType != PDR_MOTION_TYPE_STATIC) { kf->Q[0][0] = 25; // PDR预测位置噪声 kf->Q[1][1] = 25; // PDR预测位置噪声 kf->Q[2][2] = 1; // PDR步长噪声 @@ -641,49 +553,4 @@ void EKFCalQRMatrix(Nmea_t *nmea, PDR_t *g_pdr, Classifer_t *sys, kf->R[3][3] = 10000; } } -/**---------------------------------------------------------------------- -* Function : GnssInsLocFusion -* Description : PDR的GNSS和INS融合定位 -* Date : 2022/09/21 logzhan -*---------------------------------------------------------------------**/ -void GnssInsLocFusion(GNSS_t *pgnss, PDR_t *pdr, Classifer_t *sys, double yaw_bias, - EKFPara_t *kf, LctFs_t *res) { - double plla[3] = { 0 }; - double Ned[3] = { 0 }; - double yaw_thr = 6; - int scene_type = pdr->sceneType; - - plla[0] = pgnss->lat; - plla[1] = pgnss->lon; - plla[2] = 0; - - WGS842NED(plla, pdr->pllaInit, Ned); - - pgnss->xNed = Ned[0]; - pgnss->yNed = Ned[1]; - - //调整融合策略 - EKFStateUpdate(kf, pgnss, sys, pdr, scene_type); - - for (uchar i = 0; i < N; i++) { - for (uchar l = 0; l < N; l++) { - kf->pPk[i][l] = kf->Pk[i][l]; - } - kf->pXk[i] = kf->Xk[i]; - } - - Ned[0] = kf -> pXk[0] - pdr->x0; - Ned[1] = kf -> pXk[1] - pdr->y0; - Ned[2] = 0; - - NED2WGS84(pdr->pllaInit, Ned, plla); - - res->latitude = plla[0]; - res->latitude_ns = pgnss->lat_ns; - res->longitudinal = plla[1]; - res->longitudinal_ew = pgnss->lon_ew; - res->time = pgnss->timestamps; - - pdr->NoGnssCount = 0; -} diff --git a/1.Software/PDR 1.04/src/Location.c b/1.Software/PDR 1.04/src/Location.c index 926850c..1dfeb84 100644 --- a/1.Software/PDR 1.04/src/Location.c +++ b/1.Software/PDR 1.04/src/Location.c @@ -43,7 +43,7 @@ float bias_dir[2] = { 0 }; * Description : PDR导航系统初始化 * Date : 2022/09/21 logzhan *---------------------------------------------------------------------**/ -void PDRNav_Init(void) { +void PDR_Init(void) { memset(&g_kfPara, 0, sizeof(g_kfPara)); memset(&pgnss, 0, sizeof(pgnss)); @@ -112,7 +112,7 @@ int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) { pdr->gyroTime = ImuData->gyr.time; // 为什么手持类型是0.1的运动频率？ - if (pdr->motionType == DETECTOR_TYPE_HANDHELD)pdr->motionFreq = 0.1f; + if (pdr->MotionType == DETECTOR_TYPE_HANDHELD)pdr->motionFreq = 0.1f; // 计步器输入传感器数据，占10%的运行时间 PedometerUpdate(ImuData, &pdr->steps); @@ -163,15 +163,14 @@ int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) { * Description : PDR GPS融合INS惯性定位 * Date : 2021/01/29 logzhan *---------------------------------------------------------------------**/ -int GnssInsFusionLocation(Nmea_t *nmea_data, EKFPara_t *kf, LctFs_t *result) +int GnssInsFusionLocation(Nmea_t *nmea, EKFPara_t *kf, LctFs_t *result) { - // GPS数据更新 - GnssUpdate(&pgnss, nmea_data); + Nmea2Gnss(nmea, &pgnss); // 检查时间差是否满足条件 if (fabs(pgnss.timestamps - pdr->ts) >= 1000)return TYPE_FIX_NONE; // 根据nmea数据识别场景是否是开阔天空场景 - pdr->sceneType = isOpenArea(nmea_data); + pdr->sceneType = isOpenArea(nmea); // 根据HDOP、速度、卫星数量等判断gps yaw是否可用 pdr->gpsSpeed = pgnss.vel * GPS_SPEED_UNIT; @@ -183,12 +182,13 @@ int GnssInsFusionLocation(Nmea_t *nmea_data, EKFPara_t *kf, LctFs_t *result) result->pdrHeading = kf->pXk[3]; result->gpsHeading = pdr->gpsHeading; result->hdop = pgnss.HDOP; - result->accuracy = nmea_data->accuracy.err; + result->accuracy = nmea->accuracy.err; result->gpsSpeed = pdr->gpsSpeed; - result->motionType = pdr->motionType; + result->motionType = pdr->MotionType; #endif - CalPdrHeadingOffset(nmea_data, pdr); + // 利用Nmea信息修正Ins航向角 + CalPdrHeadingOffset(nmea, pdr); // 计算两次GPS时间之间，步数变化量 pdr->deltaStepsPerGps = pdr->steps - pdr->lastGpsSteps; @@ -199,16 +199,22 @@ int GnssInsFusionLocation(Nmea_t *nmea_data, EKFPara_t *kf, LctFs_t *result) //根据历史GPS拟合输出预估的GPS轨迹航向角 pdr->trackHeading = CalGnssTrackHeading(GpsPos, pgnss); + // 如果系统没有初始化，则先初始化 if (pdr->sysStatus == IS_INITIAL) { - return InitProc(&pgnss, kf, pdr, result); + // Gnss系统初始对准 + return GnssInsInitialAlignment(&pgnss, kf, pdr, result); } // 如果惯导没有达到输出条件，则不预测轨迹 - if (!pdr->fusionPdrFlg)return ResetOutputResult(&pgnss, pdr, kf, result); + if (!pdr->fusionPdrFlg) + { + return ResetOutputLoction(&pgnss, pdr, kf, result); + } // 根据GPS信息, 计算kf的过程噪声和观测噪声 - EKFCalQRMatrix(nmea_data, pdr, &sys, kf); + EKFCalQRMatrix(nmea, pdr, &sys, kf); + // 运行卡尔曼位置融合 if (pgnss.satellites_num > 4){ // 如果GPS航向角处于无效状态且速度也是无效状态 if (pgnss.yaw == INVAILD_GPS_YAW || pgnss.vel <= 0.0) { @@ -220,14 +226,14 @@ int GnssInsFusionLocation(Nmea_t *nmea_data, EKFPara_t *kf, LctFs_t *result) pgnss.yaw = (float)kf->pXk[3]; } if (pgnss.yaw != INVAILD_GPS_YAW) { - GnssInsLocFusion(&pgnss, pdr, &sys, yaw_bias, kf, result); + EkfGnssInsLocFusion(&pgnss, pdr, &sys, yaw_bias, kf, result); return TYPE_FIX_PDR; } } // 不满足推算条件，直接输出原始GPS - if (pgnss.satellites_num > 4 && nmea_data->accuracy.err > 0 && - nmea_data->accuracy.err < 15) { + if (pgnss.satellites_num > 4 && nmea->accuracy.err > 0 && + nmea->accuracy.err < 15) { OutputRawGnssPos(&pgnss, result); pdr->NoGnssCount = 0; return TYPE_FIX_GPS; @@ -276,11 +282,12 @@ void CalPdrHeadingOffset(Nmea_t* nmea_data, PDR_t* p_pdr) { if ((pgnss.satellites_num > 4 && pgnss.HDOP < 2.0 && pgnss.yaw != -1 && pgnss.vel >= 1.0) && (nmea_data->accuracy.err > 0 && nmea_data->accuracy.err < 4) - && (DetUserStatic(pdr, &pgnss, (p_pdr->steps - p_pdr->lastSteps)) == 0 && pdr->motionType == 2)) + && (DetUserStatic(pdr, &pgnss, (p_pdr->steps - p_pdr->lastSteps)) == 0 && pdr->MotionType == 2)) { /*这里放姿态角与GPS对准part1 start*/ double imuHeading = atan2(2 * (g_ahrs.qnb[0] * g_ahrs.qnb[3] + g_ahrs.qnb[1] * g_ahrs.qnb[2]), 1 - 2 * (g_ahrs.qnb[2] * g_ahrs.qnb[2] + g_ahrs.qnb[3] * g_ahrs.qnb[3])); + if (imuHeading < 0) { imuHeading += TWO_PI; } @@ -294,15 +301,15 @@ void CalPdrHeadingOffset(Nmea_t* nmea_data, PDR_t* p_pdr) { } /**---------------------------------------------------------------------- -* Function : pdr_detectFixMode +* Function : DetectFixMode * Description : 检测当前PDR处于的模式，如果是车载和静止模式,根据情况选择输出 * 原始GPS或者不输出 * Date : 2020/07/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高 * 代码的可读性 *---------------------------------------------------------------------**/ -int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, LctFs_t* result) { +int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* pdr, LctFs_t* result) { // 检测用户是否处于静止状态 - if (DetUserStatic(pdr, pgnss, g_pdr->deltaStepsPerGps)) { + if (DetUserStatic(pdr, pgnss, pdr->deltaStepsPerGps)) { if (pgnss->error > 0 && pgnss->error < 15 && pgnss->lastError >= pgnss->error) { OutputRawGnssPos(pgnss, result); @@ -372,17 +379,158 @@ void Nmea2Gnss(Nmea_t* nmea_data, GNSS_t* pgnss) pgnss->satellites_num = nmea_data->gga.satellites_nb; } + /**---------------------------------------------------------------------- -* Function : GnssUpdate -* Description : nmea数据结构转gnss数据 -* Date : 2020/07/08 logzhan -* 2020/02/09 logzhan : 函数对应原pdr_gpsUpdate函数， -* 经过引用分析发现每次GPS更新拷贝gnss结构体和nmea结构体其实没 -* 什么用，还增加运算量 +* Function : OutputRawGnssPos +* Description : 输出GPS位置 +* Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ -void GnssUpdate(GNSS_t* gps, Nmea_t* nmea) { - // nmea数据转gnss数据结构 - Nmea2Gnss(nmea, &pgnss); - // GPS回调更新，暂时不清楚函数功能 - gpsUpdateCb(gps); +void OutputRawGnssPos(GNSS_t* pgnss, LctFs_t* result) { + result->latitude = pgnss->lat; + result->latitude_ns = pgnss->lat_ns; + result->longitudinal = pgnss->lon; + result->longitudinal_ew = pgnss->lon_ew; + result->time = pgnss->timestamps; } + + +/**---------------------------------------------------------------------- +* Function : ResetOutputLoction +* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数 +* 2、GPS值赋值给result +* Date : 2022/09/19 logzhan +*---------------------------------------------------------------------**/ +int ResetOutputLoction(GNSS_t* pgnss, PDR_t* g_pdr, EKFPara_t* kf, LctFs_t* result) { + + double stepLength = 0.7; + double ned[3] = { 0 }; + + g_pdr->pllaInit[0] = pgnss->lat; + g_pdr->pllaInit[1] = pgnss->lon; + g_pdr->pllaInit[2] = 0; + WGS842NED(g_pdr->pllaInit, g_pdr->pllaInit, ned); + + g_pdr->x0 = ned[0]; + g_pdr->y0 = ned[1]; + + kf->pXk[0] = ned[0]; + kf->pXk[1] = ned[1]; + kf->pXk[2] = stepLength; + // 重置输出GPS时，GPS的航向角不一定准 + kf->pXk[3] = pgnss->yaw * RADIAN_PER_DEG; + + for (uchar i = 0; i < N; i++) { + kf->Xk[i] = kf->pXk[i]; + } + // 清除卡尔曼Q矩阵和R矩阵 + for (int i = 0; i < N; i++) { + for (int l = 0; l < N; l++) { + kf->Q[i][l] = 0.0; + kf->R[i][l] = 0.0; + } + } + // 输出GPS位置结果 + OutputRawGnssPos(pgnss, result); + g_pdr->NoGnssCount = 0; + return PDR_TRUE; +} + +/**---------------------------------------------------------------------- +* Function : EkfGnssInsLocFusion +* Description : 采用EKF对GNSS和INS融合定位 +* Date : 2022/09/21 logzhan +*---------------------------------------------------------------------**/ +void EkfGnssInsLocFusion(GNSS_t* pgnss, PDR_t* pdr, Classifer_t* sys, double yaw_bias, + EKFPara_t* kf, LctFs_t* res) { + double plla[3] = { 0 }; + double Ned[3] = { 0 }; + double yaw_thr = 6; + + int scene_type = pdr->sceneType; + + plla[0] = pgnss->lat; + plla[1] = pgnss->lon; + plla[2] = 0; + + WGS842NED(plla, pdr->pllaInit, Ned); + + pgnss->xNed = Ned[0]; + pgnss->yNed = Ned[1]; + + //调整融合策略 + EKFStateUpdate(kf, pgnss, sys, pdr, scene_type); + + for (uchar i = 0; i < N; i++) { + for (uchar l = 0; l < N; l++) { + kf->pPk[i][l] = kf->Pk[i][l]; + } + kf->pXk[i] = kf->Xk[i]; + } + + Ned[0] = kf->pXk[0] - pdr->x0; + Ned[1] = kf->pXk[1] - pdr->y0; + Ned[2] = 0; + + NED2WGS84(pdr->pllaInit, Ned, plla); + + res->latitude = plla[0]; + res->latitude_ns = pgnss->lat_ns; + res->longitudinal = plla[1]; + res->longitudinal_ew = pgnss->lon_ew; + res->time = pgnss->timestamps; + + pdr->NoGnssCount = 0; +} + +/**---------------------------------------------------------------------- +* Function : PDRInitialAlignment +* Description : PDR初始对准 +* Date : 2022/09/19 logzhan +*---------------------------------------------------------------------**/ +int GnssInsInitialAlignment(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, LctFs_t* result) +{ + double stepLen = 0.7; // 初始运动步长为0.7m + if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.5 && pgnss->yaw != -1) { + // plla 类似位置坐标的原点(单位是经纬度) + + g_pdr->pllaInit[0] = pgnss->lat; + g_pdr->pllaInit[1] = pgnss->lon; + g_pdr->pllaInit[2] = 0; + + // plla 转 ned + double ned[3] = { 0 }; + WGS842NED(g_pdr->pllaInit, g_pdr->pllaInit, ned); + + // 在NED坐标系的计算, 单位是米 + g_pdr->x0 = ned[0]; + g_pdr->y0 = ned[1]; + + // 初始化卡尔曼滤波器的观测量 + kf->pXk[0] = ned[0]; // 状态量1: 北向x + kf->pXk[1] = ned[1]; // 状态量2：东向y + kf->pXk[2] = stepLen; // 状态量3：步长 + kf->pXk[3] = pgnss->yaw * RADIAN_PER_DEG; // 状态量4：航向角 + + for (uchar i = 0; i < N; i++) { + kf->Xk[i] = kf->pXk[i]; + } + + kf->initHeading = kf->pXk[3]; + for (int i = 0; i < N; i++) { + for (int l = 0; l < N; l++) { + kf->Q[i][l] = 0.0; + kf->R[i][l] = 0.0; + } + } + OutputRawGnssPos(pgnss, result); + g_pdr->sysStatus = IS_NORMAL; + + return PDR_TRUE; + } + + if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel > 0 && pgnss->yaw != -1) { + OutputRawGnssPos(pgnss, result); + return PDR_TRUE; + } + return PDR_FALSE; +} \ No newline at end of file diff --git a/1.Software/PDR 1.04/src/Main.cpp b/1.Software/PDR 1.04/src/Main.cpp index 048d668..031d58f 100644 --- a/1.Software/PDR 1.04/src/Main.cpp +++ b/1.Software/PDR 1.04/src/Main.cpp @@ -12,9 +12,6 @@ * 度条显示轨迹。可以对比查看轨迹效果。 * * 一、优化方向和工作内容 -* 1) 要跟品质那边谈高通效果对比版本SAP5.0 -* 2）能够兼容老版本，2个月能够更新版本，能够多次迭代测试 -* 2021/02/09 logzhan ：发现PDR部分bug记录 * 1) GPS速度实际需要乘以0.51444的，但是除了滤波器计算步频之外， * 其他地方都是用的实际的GPS速度，包括步数预测 * 2) 惯性导航更新部分buffer mean使用的太多了, 计算平均没有优化 @@ -69,7 +66,7 @@ using namespace std; ResultTracks resTracks; FILE* SimFile = NULL; /* Extern Variable Definition ------------------------------------------------*/ -extern "C" PDR g_pdr; +extern "C" PDR Pdr; extern "C" double refGpsYaw; /**---------------------------------------------------------------------- @@ -121,7 +118,7 @@ int main(int argc, char* argv[]) // 解析文本仿真 while (fgets(line, 256, fileFp)){ // 解析文本数据仿真 - if (ParseLineAndUpdate(line, &locFusion) != TYPE_FIX_NONE){ + if (ParseDataAndUpdate(line, &locFusion) != TYPE_FIX_NONE){ // 更新结果轨迹，用于最后的kml生成 UpdateResTrack(resTracks, locFusion); } @@ -398,7 +395,7 @@ PosFusion LibPDR_StepUpdateGPS(int useGpsFlg) char line[256] = { 0 }; while (fgets(line, 256, SimFile)) { - int flag = ParseLineAndUpdate(line, &lct_fusion); + int flag = ParseDataAndUpdate(line, &lct_fusion); if (flag) { @@ -441,7 +438,7 @@ void setSimulatorFileCsvPath(char* path) { * Date : 2020/8/3 logzhan *---------------------------------------------------------------------**/ void setRefGpsYaw() { - refGpsYaw = g_pdr.gpsHeading; + refGpsYaw = Pdr.gpsHeading; printf("ref Gps Yaw = %f\n", refGpsYaw); //g_pdr.sysStatus = IS_INITIAL; //g_pdr.fusionPdrFlg = ON; diff --git a/1.Software/PDR 1.04/src/Munkres.c b/1.Software/PDR 1.04/src/Munkres.c index 101373b..4a0c206 100644 --- a/1.Software/PDR 1.04/src/Munkres.c +++ b/1.Software/PDR 1.04/src/Munkres.c @@ -50,9 +50,7 @@ int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) { time_algo_1 = uTimetick_Get(); #endif while (7 != step) { -#ifdef __MATLAB_DBG - mexPrintf("step %d:\n", step); -#endif + switch (step) { case 0: case 1: @@ -66,25 +64,10 @@ int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) { break; case 4: step = step_four(g_cost, g_mark, m, n, &g_row_cover, &g_col_cover, &i, &j); - #if PRINT_ALGO_RUN_TIME - time_algo_3 = uTimetick_Get(); - time_algo_3 = (time_algo_3-time_algo_1); - - #if defined(LOCATION_PLATFORM_QCOM_MODEM) - Geek_PDR_LOGH_NMEA("step_4 run time ms %d",(int)time_algo_3,0,0,0); - #endif - #endif + break; case 5: step = step_five(g_mark, m, n, &g_row_cover, &g_col_cover, i, j); - #if PRINT_ALGO_RUN_TIME - time_algo_4 = uTimetick_Get(); - time_algo_4 = (time_algo_4-time_algo_1); - - #if defined(LOCATION_PLATFORM_QCOM_MODEM) - Geek_PDR_LOGH_NMEA("step_5 run time ms %d",(int)time_algo_4,0,0,0); - #endif - #endif break; case 6: step = step_six(g_cost, m, n, &g_row_cover, &g_col_cover); @@ -93,26 +76,7 @@ int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) { step = 7; break; } -#ifdef __MATLAB_DBG - mexPrintf(" cost:\n", step); - display_cost_matrix_and_cover(g_cost, m, n, g_row_cover, g_col_cover); - mexPrintf(" mark:\n", step); - display_mark_matrix_and_cover(g_mark, m, n, g_row_cover, g_col_cover); -#endif } - #if PRINT_ALGO_RUN_TIME - time_algo_2 = uTimetick_Get(); - #endif - #if PRINT_ALGO_RUN_TIME - - time_algo_2 = (time_algo_2-time_algo_1); - // time_algo_3 = (time_algo_3-time_algo_2); - - #if defined(LOCATION_PLATFORM_QCOM_MODEM) - Geek_PDR_LOGH_NMEA("munkres run time ms %d",(int)time_algo_2,0,0,0); - #endif - // Geek_PDR_LOGH_NMEA("pdr2 run time ms %d %d",(int)time_algo_5,time_algo_6,0,0); - #endif for (i = 0; i < m; ++i) { for (j = 0; j < n; ++j) { diff --git a/1.Software/PDR 1.04/src/PDRBase.c b/1.Software/PDR 1.04/src/PDRBase.c index 045b56d..640f1a4 100644 --- a/1.Software/PDR 1.04/src/PDRBase.c +++ b/1.Software/PDR 1.04/src/PDRBase.c @@ -17,357 +17,60 @@ #include "buffer.h" #include "Filter.h" #include "pdr_base.h" -#include "Fft.h" -/* Macro Definition ----------------------------------------------------------*/ -#define DOWNSAMPLING_RATE 4 -#define BUF_LEN 256 -#define ARR_LEN(arr) (sizeof(arr) / sizeof(*arr)) -#define INC_PTR(buf, ptr) (((ptr) + 1) % ((buf)->length + 1)) -#define GET_MOL(v) ((float)sqrt((v)[0] * (v)[0] + (v)[1] * (v)[1])) -#define P_SET_BIT(msk) (g_pdr.status |= PDR_STATUS_##msk) -#define P_CLR_BIT(msk) (g_pdr.status &= ~PDR_STATUS_##msk) -#define P_TST_BIT(msk) (g_pdr.status & PDR_STATUS_##msk) /* Global Variable Definition ------------------------------------------------*/ -PDR_t g_pdr; -static Detector_t *g_detector; -static uint64_t g_tick_p; -static uint32_t g_motion_type; -static uint32_t g_axis_ceof_counter; -static BUFFER_LONG g_axis[9]; -static BUFFER_LONG g_grav[2]; -static BUFFER_LONG g_erro; -static BUFFER_LONG g_posi[4]; +PDR_t Pdr; -static int angle_count; -static double p_angle[3][BUF_LEN]; -double angle_mean[3] = { 0 }; -static Filter_t g_grv_flt[2]; - -#if defined(__MATLAB_DBG) -static int g_debug[2]; -static float axis_mean[9]; -#endif - -const double g_grv_flt_b[] = { 4.62344870721942e-10, 4.62344870721942e-09, 2.08055191824874e-08, 5.54813844866331e-08, - 9.70924228516079e-08, 1.16510907421929e-07, 9.70924228516079e-08, 5.54813844866331e-08, - 2.08055191824874e-08, 4.62344870721942e-09, 4.62344870721942e-10 }; - -const double g_grv_flt_a[] = { 1, -8.39368255078839, 31.8158646581919, -71.7026569821430, 106.381617694638, - -108.553825650279, 77.1444606240244, -37.6960546719428, 12.1197601392885, - -2.31493776310228, 0.199454975553812 }; -const double g_grv_flt_h0 = 1; - -PDR_t* Base_Init() { - g_detector = GetDetectorObj(); - BufferInit((Buffer_t *)&g_axis[0], "x_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN); - BufferInit((Buffer_t *)&g_axis[1], "x_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN); - BufferInit((Buffer_t *)&g_axis[2], "x_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN); - BufferInit((Buffer_t *)&g_axis[3], "y_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN); - BufferInit((Buffer_t *)&g_axis[4], "y_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN); - BufferInit((Buffer_t *)&g_axis[5], "y_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN); - BufferInit((Buffer_t *)&g_axis[6], "z_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN); - BufferInit((Buffer_t *)&g_axis[7], "z_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN); - BufferInit((Buffer_t *)&g_axis[8], "z_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN); - BufferInit((Buffer_t *)&g_grav[0], "grav_0", BUFFER_TYPE_QUEUE, BUF_LEN); - BufferInit((Buffer_t *)&g_grav[1], "grav_1", BUFFER_TYPE_QUEUE, BUF_LEN); - BufferInit((Buffer_t *)&g_erro, "erro", BUFFER_TYPE_QUEUE, 5 * IMU_SAMPLING_FREQUENCY / DOWNSAMPLING_RATE); - BufferInit((Buffer_t *)&g_posi[0], "posi_0", BUFFER_TYPE_QUEUE, 60); - BufferInit((Buffer_t *)&g_posi[1], "posi_1", BUFFER_TYPE_QUEUE, 60); - BufferInit((Buffer_t *)&g_posi[2], "posi_2", BUFFER_TYPE_QUEUE, 60); - BufferInit((Buffer_t *)&g_posi[3], "posi_3", BUFFER_TYPE_QUEUE, 60); - - FilterSetCoef(&g_grv_flt[0], ARR_LEN(g_grv_flt_b), (double *)g_grv_flt_b, (double *)g_grv_flt_a, (double)g_grv_flt_h0); - FilterSetCoef(&g_grv_flt[1], ARR_LEN(g_grv_flt_b), (double *)g_grv_flt_b, (double *)g_grv_flt_a, (double)g_grv_flt_h0); - - pdr_resetData(); - return &g_pdr; +PDR_t* Base_Init() +{ + ResetPDRData(); + return &Pdr; } /**---------------------------------------------------------------------- -* Function : pdr_resetData +* Function : ResetPDRData * Description : 重置PDR变量 -* Date : 2020/7/21 logzhan +* Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ -void pdr_resetData(void) { - memset(&g_pdr, 0, sizeof(g_pdr)); - g_pdr.status = PDR_STATUS_RESET; - g_pdr.axis_ceofficient[0] = 0; - g_pdr.axis_ceofficient[1] = 1; - g_pdr.axis_ceofficient[2] = 0; - g_pdr.bias_direction[0] = 1; - g_pdr.bias_direction[1] = 0; - g_pdr.pca_accuracy = -1; - g_pdr.bias_accuracy = -1; +void ResetPDRData(void) { + memset(&Pdr, 0, sizeof(Pdr)); + Pdr.status = PDR_STATUS_RESET; + Pdr.axis_ceofficient[0] = 0; + Pdr.axis_ceofficient[1] = 1; + Pdr.axis_ceofficient[2] = 0; + Pdr.bias_direction[0] = 1; + Pdr.bias_direction[1] = 0; + Pdr.pca_accuracy = -1; + Pdr.bias_accuracy = -1; // 初始坐标初始化为0 - g_pdr.x0 = 0; - g_pdr.y0 = 0; + Pdr.x0 = 0; + Pdr.y0 = 0; // 初始步数为0 - g_pdr.steps = 0; - g_pdr.lastSteps = 0; - g_pdr.lastGpsSteps = 0; - g_pdr.deltaStepsPerGps = 0; - g_pdr.heading = 0.0; - g_pdr.lastHeading = 0.0; - g_pdr.deltaHeading = 0.0; - g_pdr.ts = 0.0; - g_pdr.sysStatus = IS_INITIAL; - g_pdr.NoGnssCount = 0; - g_pdr.sceneType = UN_OPEN_AREA; - g_pdr.fusionPdrFlg = PDR_FALSE; - g_pdr.gpsHeading = 0; - g_pdr.trackHeading = ITEM_INVALID; - g_pdr.motionFreq = 0.1f; - g_pdr.insHeadingOffset = 0.0f; - g_pdr.gyroTime = 0.0; - memset(g_pdr.pllaInit, 0, sizeof(g_pdr.pllaInit)); + Pdr.steps = 0; + Pdr.lastSteps = 0; + Pdr.lastGpsSteps = 0; + Pdr.deltaStepsPerGps = 0; + Pdr.heading = 0.0; + Pdr.lastHeading = 0.0; + Pdr.deltaHeading = 0.0; + Pdr.ts = 0.0; + Pdr.sysStatus = IS_INITIAL; + Pdr.NoGnssCount = 0; + Pdr.sceneType = UN_OPEN_AREA; + Pdr.fusionPdrFlg = PDR_FALSE; + Pdr.gpsHeading = 0; + Pdr.trackHeading = ITEM_INVALID; + Pdr.motionFreq = 0.1f; + Pdr.insHeadingOffset = 0.0f; + Pdr.gyroTime = 0.0; - g_tick_p = 0; - g_motion_type = g_detector->type; - g_axis_ceof_counter = 0; - BufferClear((Buffer_t *)&g_axis[0]); - BufferClear((Buffer_t *)&g_axis[1]); - BufferClear((Buffer_t *)&g_axis[2]); - BufferClear((Buffer_t *)&g_axis[3]); - BufferClear((Buffer_t *)&g_axis[4]); - BufferClear((Buffer_t *)&g_axis[5]); - BufferClear((Buffer_t *)&g_axis[6]); - BufferClear((Buffer_t *)&g_axis[7]); - BufferClear((Buffer_t *)&g_axis[8]); - BufferClear((Buffer_t *)&g_grav[0]); - BufferClear((Buffer_t *)&g_grav[1]); - BufferClear((Buffer_t *)&g_erro); - BufferClear((Buffer_t *)&g_posi[0]); - BufferClear((Buffer_t *)&g_posi[1]); - BufferClear((Buffer_t *)&g_posi[2]); - BufferClear((Buffer_t *)&g_posi[3]); - FilterReset(&g_grv_flt[0]); - FilterReset(&g_grv_flt[1]); - - angle_count = 0; - memset(&p_angle, 0, sizeof(p_angle)); - memset(&angle_mean, 0, sizeof(angle_mean)); -} - - -static float get_main_frequency(BUFFER_SHORT *frequency, BUFFER_SHORT *amplitude, int number) { - float amp[3] = { {0.0f} }; - float frq; - int count; - int index; - int i; - - if (number > 3) { - return 0; - } - - for (i = 0; i < number; ++i) { - amp[i] = 0; - count = 0; - for (index = amplitude[i]._bottom; index != amplitude[i]._top; index = INC_PTR(&litude[i], index)) { - if (0 != amplitude[i].data[index]) { - amp[i] += amplitude[i].data[index]; - ++count; - } - } - if (0 != count) { - amp[i] /= count; - } - } - i = amp[0] > amp[1] ? 0 : 1; - i = amp[i] > amp[2] ? i : 2; - frq = 0; - count = 0; - for (index = frequency[i]._bottom; index != frequency[i]._top; index = INC_PTR(&frequency[i], index)) { - if (0 != frequency[i].data[index]) { - frq += frequency[i].data[index]; - ++count; - } - } - if (0 != count) { - frq /= count; - } - - return frq; -} - -static int get_optimal_length_by_frequency(BUFFER_LONG *buffer, float frequency) { - int temp; - - if (0 != frequency) { - temp = (int)((int)(buffer->length / (IMU_SAMPLING_FREQUENCY / frequency)) * (IMU_SAMPLING_FREQUENCY / frequency)); - return 0 == temp ? buffer->length : temp; - } - else { - return buffer->length; - } -} - -static void get_buffer_mean(float *mean, BUFFER_LONG *buffer, int length) { - int count; - int index; - int i; - - count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1); - length = count < length ? count : length; - count = 0; - memset(mean, 0, 3 * sizeof(*mean)); - for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) { - for (i = 0; i < 3; ++i) { - mean[i] += buffer[i].data[index]; - } - ++count; - } - mean[0] /= count; - mean[1] /= -count; - mean[2] /= -count; -} - -static void get_pca_direction(float *direction, BUFFER_LONG *buffer, int length) { - float buff_mean[2]; - float varX; - float varY; - float varXY; - float lambda; - int count; - int index; - int i; - - count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1); - length = count < length ? count : length; - memset(buff_mean, 0, sizeof(buff_mean)); - for (index = (buffer[0]._top + buffer[0].length + 1 - length) % (buffer[0].length + 1); index != buffer[0]._top; index = INC_PTR(&buffer[0], index)) { - for (i = 0; i < 2; ++i) { - buff_mean[i] += buffer[i].data[index]; - } - } - buff_mean[0] /= length; - buff_mean[1] /= length; - varX = 0; - varY = 0; - varXY = 0; - for (index = (buffer[0]._top + buffer[0].length + 1 - length) % (buffer[0].length + 1); index != buffer[0]._top; index = INC_PTR(&buffer[0], index)) { - float deltaX = buffer[0].data[index] - buff_mean[0]; - float deltaY = buffer[1].data[index] - buff_mean[1]; - varX += deltaX * deltaX; - varY += deltaY * deltaY; - varXY += deltaX * deltaY; - } - lambda = ((varX + varY) + (float)sqrt((varX - varY) * (varX - varY) + 4 * varXY * varXY)) * 0.5f; - if (0 != varXY) { - direction[0] = (lambda - varY) / varXY; - direction[1] = InvSqrt(direction[0] * direction[0] + 1); - direction[0] *= direction[1]; - direction[1] = -direction[1]; - } - else { - direction[0] = 0; - direction[1] = 0; - } -} - -static void caculate_determinant(float *determinant, float A[][3], int row, int col) { - int k = 0; - float temp[4]; - for (uchar i = 0; i < 3; ++i) { - if (row == i) continue; - for (uchar j = 0; j < 3; ++j) { - if (col == j) continue; - temp[k++] = A[i][j]; - } - } - *determinant = temp[0] * temp[3] - temp[1] * temp[2]; -} - -static void caculate_adjoint_matrix(float A_star[][3], float A[][3]) { - int i; - int j; - - for (i = 0; i < 3; ++i) { - for (j = 0; j < 3; ++j) { - caculate_determinant(&A_star[i][j], A, j, i); - if ((i + j) % 2) { - A_star[i][j] = -A_star[i][j]; - } - } - } -} - -static void get_ceof_by_pca(float *ceof, float *axis_mean, float *pca_direction) { - float A[3][3]; - float A_star[3][3]; - float x[3]; - float axs_dir[2]; - float mold; - int dir; - int i; - int j; - - /* caculate adjoint matrix */ - for (i = 0; i < 3; ++i) { - for (j = 0; j < 3; ++j) { - A[i][j] = axis_mean[j * 3 + i]; - } - } - caculate_adjoint_matrix(A_star, A); - - /* caculate x vector (Ax = b) */ - for (i = 0; i < 3; ++i) { - x[i] = A_star[i][0] * pca_direction[0] + A_star[i][1] * pca_direction[1]; - } - mold = (float)sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]); - if (0 == mold) { - goto GET_CEOF_BY_PCA_ARITHMETIC_ERROR; - } - for (i = 0; i < 3; ++i) { - x[i] /= mold; - } - - for (i = 0; i < 2; ++i) { - axs_dir[i] = A[i][0] * ceof[0] + A[i][1] * ceof[1] + A[i][2] * ceof[2]; - } - dir = (axs_dir[0] * pca_direction[0] + axs_dir[1] * pca_direction[1] < 0) ? -1 : 1; - - for (i = 0; i < 3; ++i) { - ceof[i] = dir * x[i]; - } - - return; - -GET_CEOF_BY_PCA_ARITHMETIC_ERROR: - ceof[0] = 0; - ceof[1] = 1; - ceof[2] = 0; -} - -static void correct_axis_ceofficient(float ratio) { - float axis_mean[9]; - float pca_direction[2]; - float axis_ceofficient[3]; - float mold; - int length; - int i; - - length = get_optimal_length_by_frequency(g_axis, g_pdr.motionFreq); - get_buffer_mean(&axis_mean[0], &g_axis[0], length); - get_buffer_mean(&axis_mean[3], &g_axis[3], length); - get_buffer_mean(&axis_mean[6], &g_axis[6], length); - get_pca_direction(pca_direction, g_grav, length); - memcpy((void *)axis_ceofficient, (void *)g_pdr.axis_ceofficient, sizeof(g_pdr.axis_ceofficient)); - get_ceof_by_pca(axis_ceofficient, axis_mean, pca_direction); - for (i = 0; i < 3; ++i) { - g_pdr.axis_ceofficient[i] = (1 - ratio) * g_pdr.axis_ceofficient[i] + ratio * axis_ceofficient[i]; - } - mold = (float)sqrt(g_pdr.axis_ceofficient[0] * g_pdr.axis_ceofficient[0] + g_pdr.axis_ceofficient[1] * g_pdr.axis_ceofficient[1] + g_pdr.axis_ceofficient[2] * g_pdr.axis_ceofficient[2]); - for (i = 0; i < 3; ++i) { - g_pdr.axis_ceofficient[i] /= mold; - } + memset(Pdr.pllaInit, 0, sizeof(Pdr.pllaInit)); } /**---------------------------------------------------------------------- -* Function : pdr_computePCA +* Function : ComputePCA * Description : 进入RESET_PHASE_1\STABLE阶段之后进行PCA计算 * Date : 2020/7/21 logzhan *---------------------------------------------------------------------**/ @@ -376,24 +79,10 @@ void ComputePCA(AHRS_t *ahrs) { } /**---------------------------------------------------------------------- -* Function : OutputRawGnssPos -* Description : 输出GPS位置 -* Date : 2022/10/15 logzhan -*---------------------------------------------------------------------**/ -void OutputRawGnssPos(GNSS_t* pgnss, LctFs_t* result) { - result->latitude = pgnss->lat; - result->latitude_ns = pgnss->lat_ns; - result->longitudinal = pgnss->lon; - result->longitudinal_ew = pgnss->lon_ew; - result->time = pgnss->timestamps; -} - - -/**---------------------------------------------------------------------- -* Function : pdr_detStatic +* Function : DetUserStatic * Description : 检测用户是否处于静止状态, 目前主要采用计步器判断和运动频率分析 * 单运动频率计算量太大，可以考虑用加速度判断用户状态。 -* Date : 2021/01/28 logzhan +* Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ int DetUserStatic(PDR_t* g_pdr, GNSS_t* pgnss, unsigned long delSteps) { @@ -414,9 +103,10 @@ int DetUserStatic(PDR_t* g_pdr, GNSS_t* pgnss, unsigned long delSteps) { * Output : int，车载模式标志位 * Date : 2021/01/28 logzhan *---------------------------------------------------------------------**/ -int DetectCarMode(GNSS_t* pgnss, PDR_t* pdr, unsigned long delSteps, int* time) { - if ((pgnss->vel > 10 && delSteps == 0) || - (delSteps == 0 && pdr->motionFreq == 0.0 && pgnss->vel > 3 && pgnss->error < 30)) +int DetectCarMode(GNSS_t* gnss, PDR_t* pdr, unsigned long delSteps, int* time) +{ + if ((gnss->vel > 10 && delSteps == 0) || + (delSteps == 0 && pdr->motionFreq == 0.0 && gnss->vel > 3 && gnss->error < 30)) { (*time)++; }else { @@ -434,14 +124,14 @@ int DetectCarMode(GNSS_t* pgnss, PDR_t* pdr, unsigned long delSteps, int* time) * 重置PDR到GPS的位置。 * Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ -int DetectPdrToReset(double pdr_angle, int* gpscnt, ulong deltsteps, PDR_t* g_pdr) { +int DetectPdrToReset(double pdr_angle, int* gpsCnt, ulong deltsteps, PDR_t* pdr) { - if ((pdr_angle < 0 && deltsteps > 0) || (deltsteps > 0 && (g_pdr->motionFreq == 0))) { - (*gpscnt)++; + if ((pdr_angle < 0 && deltsteps > 0) || (deltsteps > 0 && (pdr->motionFreq == 0))) { + (*gpsCnt)++; }else { - *gpscnt = 0; + *gpsCnt = 0; } - return (*gpscnt > 6); + return (*gpsCnt > 6); } /**--------------------------------------------------------------------- @@ -449,140 +139,7 @@ int DetectPdrToReset(double pdr_angle, int* gpscnt, ulong deltsteps, PDR_t* g_pd * Description : 根据新确定的手机携带方式更新状态等 * Date : 2022/10/15 logzhan *---------------------------------------------------------------------**/ -void DetectorUpdate(Detector_t *detector) { - +void DetectorUpdate(Detector_t *Detector) { // 设置PDR的运动类型 - g_pdr.motionType = detector->type; - - if (DETECTOR_TYPE_SWINGING == g_pdr.motionType && DETECTOR_TYPE_SWINGING != g_motion_type) { - if (!P_TST_BIT(STABLE)) { - P_SET_BIT(CACULATING_AXIS_CEOFFICIENT); - P_CLR_BIT(STABLE); - } - } - else if ((DETECTOR_TYPE_STATIC == g_pdr.motionType || DETECTOR_TYPE_IRREGULAR == g_pdr.motionType) && - (DETECTOR_TYPE_HANDHELD == g_motion_type || DETECTOR_TYPE_SWINGING == g_motion_type)) { - P_CLR_BIT(CACULATING_AXIS_CEOFFICIENT); - P_CLR_BIT(STABLE); - } - - g_motion_type = g_pdr.motionType; + Pdr.MotionType = Detector->type; } - -/**--------------------------------------------------------------------- -* Function : gpsUpdateCb -* Description : gps更新回调函数，目前初步来看主要是计算PCA和GPS方向的区别， -* 分析PCA方向误差作用。 -* Date : 2020/02/16 logzhan -*---------------------------------------------------------------------**/ -void gpsUpdateCb(GNSS_t* gps) -{ - float temp; - float meanA[2]; - float meanY[2]; - float ctc; // ctc[0][0], ctc[1][1] - float cty[2]; - float a[2]; - float mold; - float dx; - float dy; - int count; - int index; - - /* filter bad point */ - double vel_thr; - double error_thr; - if (g_pdr.sceneType) { - vel_thr = 2.5; - error_thr = 6; - } - else { - vel_thr = 1.0; - error_thr = 15; - } - - if (fabs(gps->timestamps - g_pdr.gyroTime) > 1000 - || gps->vel < vel_thr \ - || gps->error > error_thr \ - || (DETECTOR_TYPE_SWINGING != g_detector->type && DETECTOR_TYPE_HANDHELD != g_detector->type) \ - || -1 == g_pdr.pca_accuracy \ - || gps->yaw < 0) { - return; - } - - /* caculate position */ - BufferGetTop((Buffer_t *)&g_posi[0], &temp); - BufferPush((Buffer_t *)&g_posi[0], temp + (float)cos(gps->yaw / 180 * M_PI)); - BufferGetTop((Buffer_t *)&g_posi[1], &temp); - BufferPush((Buffer_t *)&g_posi[1], temp + (float)sin(gps->yaw / 180 * M_PI)); - BufferGetTop((Buffer_t *)&g_posi[2], &temp); - BufferPush((Buffer_t *)&g_posi[2], temp + g_pdr.pca_direction[0]); - BufferGetTop((Buffer_t *)&g_posi[3], &temp); - BufferPush((Buffer_t *)&g_posi[3], temp + g_pdr.pca_direction[1]); - if ((g_posi[0]._top - g_posi[0]._bottom + g_posi[0].length + 1) % (g_posi[0].length + 1) >= 5) { - - count = 0; - meanA[0] = 0; - meanA[1] = 0; - meanY[0] = 0; - meanY[1] = 0; - for (index = g_posi[0]._bottom; index != g_posi[0]._top; index = INC_PTR(&g_posi[0], index)) { - meanA[0] += g_posi[0].data[index]; - meanA[1] += g_posi[1].data[index]; - meanY[0] += g_posi[2].data[index]; - meanY[1] += g_posi[3].data[index]; - ++count; - } - meanA[0] /= count; - meanA[1] /= count; - meanY[0] /= count; - meanY[1] /= count; - - for (index = g_posi[0]._bottom; index != g_posi[0]._top; index = INC_PTR(&g_posi[0], index)) { - g_posi[0].data[index] -= meanA[0]; - g_posi[1].data[index] -= meanA[1]; - g_posi[2].data[index] -= meanY[0]; - g_posi[3].data[index] -= meanY[1]; - } - - ctc = 0; - cty[0] = 0; - cty[1] = 0; - for (index = g_posi[0]._bottom; index != g_posi[0]._top; index = INC_PTR(&g_posi[0], index)) { - ctc += g_posi[0].data[index] * g_posi[0].data[index] + g_posi[1].data[index] * g_posi[1].data[index]; - cty[0] += g_posi[0].data[index] * g_posi[2].data[index] + g_posi[1].data[index] * g_posi[3].data[index]; - cty[1] += -g_posi[1].data[index] * g_posi[2].data[index] + g_posi[0].data[index] * g_posi[3].data[index]; - } - - if (0 != ctc) { - /* (c^Tc)^{-1}c^Ty */ - a[0] = cty[0] / ctc; - a[1] = cty[1] / ctc; - } - else { - goto GPS_UPDATE_ARITHMETIC_ERROR; - } - - if (0 == (mold = (float)GET_MOL(a))) { - goto GPS_UPDATE_ARITHMETIC_ERROR; - } - g_pdr.bias_direction[0] = a[0] / mold; - g_pdr.bias_direction[1] = a[1] / mold; - g_pdr.bias_accuracy = 0; - for (index = g_posi[0]._bottom; index != g_posi[0]._top; index = INC_PTR(&g_posi[0], index)) { - dx = g_posi[2].data[index] - a[0] * g_posi[0].data[index] + a[1] * g_posi[1].data[index]; - dy = g_posi[3].data[index] - a[1] * g_posi[0].data[index] - a[0] * g_posi[1].data[index]; - g_pdr.bias_accuracy += dx * dx + dy * dy; - } - g_pdr.bias_accuracy = (float)sqrt(g_pdr.bias_accuracy / count); - P_SET_BIT(BIAS_STABLE); - - return; - } - -GPS_UPDATE_ARITHMETIC_ERROR: - g_pdr.bias_direction[0] = 1; - g_pdr.bias_direction[1] = 0; - g_pdr.bias_accuracy = -1; - P_CLR_BIT(BIAS_STABLE); -} \ No newline at end of file