logzhan
/
PDR
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

整理计步器

PDRHistoryBranch
詹力 2023-02-25 22:43:50 +08:00
parent 5bd0ca97f2
commit d20b742a36
51 changed files with 9729 additions and 377 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

139
1.Software/PDR 1.05/include/Pedometer.h
View File

@ -1,25 +1,10 @@
#pragma once
/******************** (C) COPYRIGHT 2017 Geek************************************
* File Name : Steps_Algorithm.h
/******************** (C) COPYRIGHT 2023 Geek************************************
* File Name : Pedometer.h
* Department : Sensor Team
* Current Version : V1.0
* Author : Xiaoyong Li
* Date of Issued : 2017.05.8
* Comments: step counter algorithm added
********************************************************************************
* THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR REFERENCE OR
* EDUCATION. AS A RESULT, Geek SOFTWARE SHALL NOT BE HELD LIABLE FOR ANY
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
* FROM THE CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
********************************************************************************
* Release Log
*
* Comments: step counter algorithm released
* Version Number : V1.0
* Author : Xiaoyong Li
* Date of Issued : 2017.05.8
* Current Version : V1.0.0
* Author : logzhan
* Date of Issued : 2023.02.24
*******************************************************************************/
#ifdef __cplusplus
@ -29,66 +14,45 @@ extern "C" {
#define _STEPS_ALGORITHM_H_
#include "pdr_sensor.h"
#define ACC_SAMPLE_RATE_100HZ
//#define ACC_SAMPLE_RATE_25HZ
#define IMU_SAMPLE_FREQ 100 // IMU采样频率100Hz
#define MS2S 1000 // 秒转毫秒单位
#ifdef ACC_SAMPLE_RATE_25HZ
#define DownResampleTimes 1
#define ACC_SAMPLE_TIME 40
#define SAMPLE_TIME_MIN 36
#else
#define DOWN_SAMPLE_TIMES 4
#define ACC_SAMPLE_TIME 10 //origin acc sampleFrequency is 100 hz
#define GYRO_SAMPLE_TIME 10
#define SAMPLE_TIME_MIN 9
#endif
#define DOWN_SAMPLE_NUM 4 // 降采样数量
#define ACC_SAMPLE_TIME MS2S / IMU_SAMPLE_FREQ // ACC采样间隔10ms
#define GYRO_SAMPLE_TIME MS2S / IMU_SAMPLE_FREQ // GYR采样间隔10ms
#define SAMPLE_TIME_MIN 9
#define SampleFrequency 25
#define SampleFrequencyFour 100
#define SampleFrequencydouble 50
#define SampleFrequencyOPF (37.5l)
#define SampleFrequencySix 150
#define AvergeFilterOrder 5
#define AxisNumber 4
#define FiltDifNumber 4
#define AxisNumberFiltOut 16 //(AxisNumber * FiltDifNumber)
#define AxisNumberStepSaveState 20 //(AxisNumberFiltOut + FiltDifNumber)
#define AccelerationNumber 39
#define ORI_ACC_SAMPLE_RATE 100 //origin acc sampleFrequency is 100 hz
#define WIN_LENGTH_AVE_DEVI 50 //
#define PI2A (180/3.1415926)
//#define NSTOS 1000000000
#define MS2S 1000
#define PEDOMETER_FREQ IMU_SAMPLE_FREQ / DOWN_SAMPLE_NUM // 计步器的频率是
#define SampleFrequencyFour 100
#define SampleFrequencydouble 50
#define SampleFrequencyOPF (37.5l)
#define SampleFrequencySix 150
#define AvergeFilterOrder 5
#define AXIS_NUM 4 // 轴的数量有4个: x、y、z、xyz
#define FILTER_NUM 4
#define FiltDifNumber 4
#define FILTER_AXIS_NUM AXIS_NUM * FILTER_NUM // 轴数量4，滤波器数量4，总共16轴滤波器输出
#define AxisNumberStepSaveState 20 // AxisNumberFiltOut + FiltDifNumber
#define ACC_BUFF_NUM 39
#define WIN_LENGTH_AVE_DEVI 50 //
#define PI2A (180/3.1415926)
#define STARTSTEPS 8
#define ACC_INSPECT_TIMEWIDTH 200 //origin acc sampleFrequency is 100 hz
#define ACC_INSPECT_TIMEWIDTH 200 // Origin acc sampleFrequency is 100 hz
#define WINDOW_LENGTH_MIN2MAX (ACC_INSPECT_TIMEWIDTH/2 - 1) //25hz , 1 second
//#define WINDOW_LENGTH_IN_VEHICLE
#define STARTSTEPS 8
/*#define PLATFORM_ANDROID_QCOM_AP*/
/*#define PLATFORM_ANDROID_QCOM_ADSP*/
#define PLATFORM_PC_WINDOWS
//#define PLATFORM_PC_LINUX
#if defined PLATFORM_ANDROID_QCOM_ADSP
#define STEPLIB_MEMSET SNS_OS_MEMSET
#if 1
#define SAM_STEP_LIB_NAME "steplib"
#define SAM_STEP_LIB_MSG_0(msg) UMSG(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg)
#define SAM_STEP_LIB_MSG_1(msg,p1) UMSG_1(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1)
#define SAM_STEP_LIB_MSG_2(msg,p1,p2) UMSG_2(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1,p2)
#define SAM_STEP_LIB_MSG_3(msg,p1,p2,p3) UMSG_3(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1,p2,p3)
#define SAM_STEP_LIB_MSG_4(msg,p1,p2,p3,p4) UMSG_4(MSG_SSID_SNS,DBG_ERROR_PRIO, SAM_STEP_LIB_NAME" - "msg,p1,p2,p3,p4)
#else
#define SAM_STEP_LIB_MSG_0(msg)
#define SAM_STEP_LIB_MSG_1(msg,p1)
#define SAM_STEP_LIB_MSG_2(msg,p1,p2)
#define SAM_STEP_LIB_MSG_3(msg,p1,p2,p3)
#define SAM_STEP_LIB_MSG_4(msg,p1,p2,p3,p4)
#endif
#elif defined PLATFORM_ANDROID_QCOM_AP || defined PLATFORM_PC_WINDOWS || defined PLATFORM_PC_LINUX || defined PLATFORM_ANDROID_MTK || defined PLATFORM_ANDROID_QCOM_SLPI845
#define STEPLIB_MEMSET memset
@ -108,38 +72,39 @@ typedef struct {
double walk;
double walkRealTime;
double nD;
}stepsDfine;
}StepsDefine_t;
typedef enum {
nonSteps = 0,
walk,
}enumState;
run,
}StepState_t;
typedef struct {
unsigned char sign;
enumState state;
StepState_t state;
unsigned long long stateStartTime;
unsigned long long stepsStartTime;
}stateNowDfine;
typedef struct {
enumState state;
StepState_t state;
unsigned long long startTimeStamp;
}stateDfine;
}StateDef_t;
typedef struct {
int flag;
double ax;
double ay;
double az;
}downSample;
}DownSample_t;
/**---------------------------------------------------------------------
* Function : getPedometerLibVersion
* Function : Pedometer_GetVersion
* Description : 获取计步器版本
* Date : 2020/2/20
* Date : 2023/02/24
*---------------------------------------------------------------------**/
const char* GetPedometerVersion(void);
const char* Pedometer_GetVersion(void);
/**---------------------------------------------------------------------
* Function : initPedometer
@ -149,28 +114,28 @@ const char* GetPedometerVersion(void);
void Pedometer_Init(void);
/**---------------------------------------------------------------------
* Function : reinitPedometer
* Description : PDR ÖØгõʼ»¯¼Æ²½Æ÷
* Date : 2020/2/20 logzhan
* Function : Pedometer_ReInit
* Description : 重新初始化计步器
* Date : 2023/02/24 logzhan
*---------------------------------------------------------------------**/
void reinitPedometer(void);
void Pedometer_ReInit(void);
/**---------------------------------------------------------------------
* Function : updatePedometer
* Function : PedometerUpdate
* Description : PDR 计步器的加速度输入, 并通过指针返回用户步数
* Date : 2020/2/1 logzhan
* Date : 2023/02/25 logzhan 重构函数
*---------------------------------------------------------------------**/
void PedometerUpdate(IMU_t* ss_data, unsigned long* step);
void Steps_State_Detect(enumState* state);
void Pedometer_Update(IMU_t* ss_data, unsigned long* step);
void Pedometer_GetState(StepState_t* state);
/**---------------------------------------------------------------------
* Function : detVehicleMode
* Function : DetVehicleMode
* Description : PDR 检测汽车模式, 如果是检测到汽车模式，那么不计步数
* Date : 2020/2/1 logzhan
* Date : 2023/02/24 logzhan
*---------------------------------------------------------------------**/
void detVehicleMode(double ax, double ay, double az);
void DetVehicleMode(double ax, double ay, double az);
#endif
#ifdef __cplusplus
}

2
1.Software/PDR 1.05/src/Location.c
View File

@ -134,7 +134,7 @@ int InsLocationUpdate(IMU_t* ImuData, EKFPara_t* Ekf) {
if (pdr.MotionType == DETECTOR_TYPE_HANDHELD)pdr.motionFreq = 0.1f;
// 计步器输入传感器数据，占10%的运行时间
PedometerUpdate(ImuData, &pdr.steps);
Pedometer_Update(ImuData, &pdr.steps);
// ----- Filter Initialization ------- //
if (pdr.sysStatus == IS_INITIAL) {

498
1.Software/PDR 1.05/src/Pedometer.c
View File

@ -14,62 +14,57 @@
const static char* Pedometer_Version = "Pedometer.1.0.0.20230224";
static void Steps_Acc_Save(double ax, double ay, double az);
static void AccBufferUpdate(double ax, double ay, double az);
static void State_Detect(unsigned long long timeStamp);
static void Steps_Filt(void);
static void Pedometer_Filter(void);
static void Steps_Feature(void);
static void Steps_Rcording(unsigned long long timeStamp);
static double stepsFiltAcc[AxisNumber][AccelerationNumber];
static double AccBuffer[AXIS_NUM][ACC_BUFF_NUM];
static float AccData[ACC_INSPECT_TIMEWIDTH] = { 0 };
static float lastAccZ = 0;
static unsigned int ReceivedHowManyAcc = 0;
static double SumAcc = 0;
static double DifFiltCoefficientOne[5] = { 0.0123456790123457, 0.0493827160493827, 0.123456790123457, 0.197530864197531, 0.234567901234568 };
static double DifFiltCoefficientTwo[9] = { 0.0016, 0.0064, 0.016, 0.032, 0.056, 0.0832, 0.1088, 0.128, 0.136 };
static double DifFiltCoefficientThree[13] = { 0.000416493127863390, 0.00166597251145356, 0.00416493127863390, 0.00832986255726780, 0.0145772594752187, 0.0233236151603498, 0.0349854227405248, 0.0483132028321533, 0.0620574760516451, 0.0749687630154102, 0.0857975843398584, 0.0932944606413994, 0.0962099125364431 };
static double DifFiltCoefficientFour[17] = { 0.000152415790275873, 0.000609663161103490, 0.00152415790275873, 0.00304831580551745, 0.00533455265965554, 0.00853528425544886, 0.0128029263831733, 0.0182898948331047, 0.0251486053955190, 0.0329218106995885, 0.0411522633744856, 0.0493827160493827, 0.0571559213534522, 0.0640146319158665, 0.0695016003657979, 0.0731595793324188, 0.0745313214449017 };
static double DifFiltCoefficient1[5] = { 0.0123456790123457, 0.0493827160493827, 0.123456790123457, 0.197530864197531, 0.234567901234568 };
static double DifFiltCoefficient2[9] = { 0.0016, 0.0064, 0.016, 0.032, 0.056, 0.0832, 0.1088, 0.128, 0.136 };
static double DifFiltCoefficient3[13] = { 0.000416493127863390, 0.00166597251145356, 0.00416493127863390, 0.00832986255726780, 0.0145772594752187, 0.0233236151603498, 0.0349854227405248, 0.0483132028321533, 0.0620574760516451, 0.0749687630154102, 0.0857975843398584, 0.0932944606413994, 0.0962099125364431 };
static double DifFiltCoefficient4[17] = { 0.000152415790275873, 0.000609663161103490, 0.00152415790275873, 0.00304831580551745, 0.00533455265965554, 0.00853528425544886, 0.0128029263831733, 0.0182898948331047, 0.0251486053955190, 0.0329218106995885, 0.0411522633744856, 0.0493827160493827, 0.0571559213534522, 0.0640146319158665, 0.0695016003657979, 0.0731595793324188, 0.0745313214449017 };
static double DifFiltOutput[AxisNumberFiltOut][5];
static double OutputFilt[AxisNumberFiltOut][2];
static double StepLcVar[AxisNumberFiltOut][3];
static char StepReFeature[AxisNumberFiltOut] = { 0 };
static double StepFetureAmp[AxisNumberFiltOut][8];
static unsigned short StepFetureTime[AxisNumberFiltOut][8];
static double DifFiltOutput[FILTER_AXIS_NUM][5];
static double OutputFilt[FILTER_AXIS_NUM][2];
static double StepLcVar[FILTER_AXIS_NUM][3];
static char StepReFeature[FILTER_AXIS_NUM] = { 0 };
static double StepFetureAmp[FILTER_AXIS_NUM][8];
static unsigned short StepFetureTime[FILTER_AXIS_NUM][8];
static double StepRecordeState[AxisNumberStepSaveState][4];
static double StepRecordeStateTime[AxisNumberStepSaveState][9];
static unsigned long long timeStampSave[AxisNumberStepSaveState][2];
static unsigned short StepMaxTime = 100;
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD>IJ<EFBFBD><C4B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ40ms,25HZ*/
static unsigned short StepMinTime = 10;
static unsigned long long timeStamp = 0;
static unsigned long long timeStamp_in_vehicle = 0;
static char Flag_whether_in_vehicle = 0;
static char Flag_step_exist = 0;
static double AvergeFiltSave[AxisNumber - 1][AvergeFilterOrder]/* = { 0 }*/;
stepsDfine Steps = { 0, 0 };
stepsRe ReSteps = { 0 };
downSample DownSample = { 0, 0, 0, 0 };
static double StateDetectLcvar[5] = { 0 };
static enumState stateLcVar[2] = { nonSteps, nonSteps };
static double AvergeFiltSave[AXIS_NUM - 1][AvergeFilterOrder];
StepsDefine_t Steps = { 0, 0 };
stepsRe ReSteps = { 0 };
DownSample_t DownSample = { 0, 0, 0, 0 };
static double StateDetectLcVar[5] = { 0 };
static StepState_t stateLcVar[2] = { nonSteps, nonSteps };
stateNowDfine stateNow;
stateDfine Restate;
StateDef_t Restate;
/**---------------------------------------------------------------------
* Function : GetPedometerVersion
* Function : Pedometer_GetVersion
* Description : 获取计步器版本
* Date : 2023/02/24
*---------------------------------------------------------------------**/
const char* GetPedometerVersion(void)
{
const char* Pedometer_GetVersion(void){
return Pedometer_Version;
}
@ -80,7 +75,7 @@ const char* GetPedometerVersion(void)
*---------------------------------------------------------------------**/
void Pedometer_Init(void)
{
STEPLIB_MEMSET(stepsFiltAcc, 0, sizeof(stepsFiltAcc));
STEPLIB_MEMSET(AccBuffer, 0, sizeof(AccBuffer));
STEPLIB_MEMSET(DifFiltOutput, 0, sizeof(DifFiltOutput));
STEPLIB_MEMSET(OutputFilt, 0, sizeof(OutputFilt));
STEPLIB_MEMSET(StepLcVar, 0, sizeof(StepLcVar));
@ -95,7 +90,7 @@ void Pedometer_Init(void)
timeStamp = 0;
STEPLIB_MEMSET(&Steps, 0, sizeof(Steps));
STEPLIB_MEMSET(&ReSteps, 0, sizeof(ReSteps));
STEPLIB_MEMSET(StateDetectLcvar, 0, sizeof(StateDetectLcvar));
STEPLIB_MEMSET(StateDetectLcVar, 0, sizeof(StateDetectLcVar));
STEPLIB_MEMSET(stateLcVar, 0, sizeof(stateLcVar));
STEPLIB_MEMSET(&stateNow, 0, sizeof(stateNow));
STEPLIB_MEMSET(&Restate, 0, sizeof(Restate));
@ -113,42 +108,41 @@ void Pedometer_Init(void)
* Description : PDR 重新初始化计步器
* Date : 2020/2/20 logzhan
*---------------------------------------------------------------------**/
void reinitPedometer(void)
void Pedometer_ReInit(void)
{
timeStamp = 0;
STEPLIB_MEMSET(&Steps, 0, sizeof(Steps));
STEPLIB_MEMSET(&ReSteps, 0, sizeof(ReSteps));
}
/**---------------------------------------------------------------------
* Function : updatePedometer
* Function : Pedometer_Update
* Description : PDR 计步器的加速度输入, 并通过指针返回用户步数
* Date : 2020/2/1 logzhan
* Date : 2023/02/25 logzhan
*---------------------------------------------------------------------**/
void PedometerUpdate(IMU_t* ss_data, unsigned long* step)
void Pedometer_Update(IMU_t* imu, unsigned long* step)
{
unsigned char k = 0;
// 检测当前是否属于开车状态, 这个函数也是相对耗时的, 这个函数只用
// 了加速度计的z轴, 是不是存在问题？
detVehicleMode(ss_data->acc.s[0], ss_data->acc.s[1], ss_data->acc.s[2]);
DetVehicleMode(imu->acc.s[0], imu->acc.s[1], imu->acc.s[2]);
timeStamp += ACC_SAMPLE_TIME;
DownSample.ax += ss_data->acc.s[0] / 9.8;
DownSample.ay += ss_data->acc.s[1] / 9.8;
DownSample.az += ss_data->acc.s[2] / 9.8;
DownSample.ax += imu->acc.s[0] / 9.8;
DownSample.ay += imu->acc.s[1] / 9.8;
DownSample.az += imu->acc.s[2] / 9.8;
DownSample.flag++;
if (DownSample.flag == DOWN_SAMPLE_TIMES)
if (DownSample.flag == DOWN_SAMPLE_NUM)
{
if (Restate.startTimeStamp == 0)
{
Restate.startTimeStamp = timeStamp;
}
DownSample.ax /= DOWN_SAMPLE_TIMES;
DownSample.ay /= DOWN_SAMPLE_TIMES;
DownSample.az /= DOWN_SAMPLE_TIMES;
DownSample.ax /= DOWN_SAMPLE_NUM;
DownSample.ay /= DOWN_SAMPLE_NUM;
DownSample.az /= DOWN_SAMPLE_NUM;
for (k = 0; k < AvergeFilterOrder - 1; k++)
{
AvergeFiltSave[0][k] = AvergeFiltSave[0][k + 1];
@ -170,15 +164,15 @@ void PedometerUpdate(IMU_t* ss_data, unsigned long* step)
DownSample.ay /= AvergeFilterOrder;
DownSample.az /= AvergeFilterOrder;
// 计步器的加速度更新
Steps_Acc_Save(DownSample.ax, DownSample.ay, DownSample.az);
// 计步器缓存Buffer更新
AccBufferUpdate(DownSample.ax, DownSample.ay, DownSample.az);
// 计步器状态检测
State_Detect(timeStamp);
if (StateDetectLcvar[4] < 0.5l)
if (StateDetectLcVar[4] < 0.5l)
{
// 滤波处理
Steps_Filt();
Pedometer_Filter();
// 计步特征
Steps_Feature();
// 步数记录
@ -189,7 +183,8 @@ void PedometerUpdate(IMU_t* ss_data, unsigned long* step)
}
// 如果计步标志位不存在，那么不计步数
if (Flag_step_exist == 0){
timeStamp = timeStamp + 1; //vehicle for debug
// vehicle for debug
timeStamp = timeStamp + 1;
}
else if (llabs((long long)(timeStamp - timeStamp_in_vehicle)) > 500)
{
@ -204,7 +199,7 @@ void PedometerUpdate(IMU_t* ss_data, unsigned long* step)
* Description : PDR 检测汽车模式, 如果是检测到汽车模式，那么不计步数
* Date : 2020/2/1 logzhan
*---------------------------------------------------------------------**/
void detVehicleMode(double ax, double ay, double az)
void DetVehicleMode(double ax, double ay, double az)
{
int t;
int index_move;
@ -291,156 +286,101 @@ void detVehicleMode(double ax, double ay, double az)
}
static void Steps_Acc_Save(double ax, double ay, double az)
/**---------------------------------------------------------------------
* Function : AccBufferUpdate
* Description : 更新Acc的Buffer缓存
* Date : 2023/02/25
*---------------------------------------------------------------------**/
static void AccBufferUpdate(double ax, double ay, double az)
{
short i, j;
for (i = 0; i < AxisNumber; i++){
for (j = 0; j < (AccelerationNumber - 1); j++){
stepsFiltAcc[i][j] = stepsFiltAcc[i][j + 1];
for (uint8_t i = 0; i < AXIS_NUM; i++){
for (uint8_t j = 0; j < (ACC_BUFF_NUM - 1); j++){
AccBuffer[i][j] = AccBuffer[i][j + 1];
}
}
stepsFiltAcc[0][AccelerationNumber - 1] = ax;
stepsFiltAcc[1][AccelerationNumber - 1] = ay;
stepsFiltAcc[2][AccelerationNumber - 1] = az;
stepsFiltAcc[3][AccelerationNumber - 1] = (sqrt(ax*ax + ay*ay + az*az));
AccBuffer[0][ACC_BUFF_NUM - 1] = ax;
AccBuffer[1][ACC_BUFF_NUM - 1] = ay;
AccBuffer[2][ACC_BUFF_NUM - 1] = az;
AccBuffer[3][ACC_BUFF_NUM - 1] = sqrt(ax*ax + ay*ay + az*az);
}
/*
DifFiltOutput <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16*5
OutputFilt <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16*2
0 filter one and acc x
1 filter one and acc y
2 filter one and acc z
3 filter one and acc xyz
4 filter two and acc x
5 filter two and acc y
6 filter two and acc z
7 filter two and acc xyz
8 filter three and acc x
9 filter three and acc y
10 filter three and acc z
11 filter three and acc xyz
12 filter four and acc x
13 filter four and acc y
14 filter four and acc z
15 filter four and acc xyz
*/
static void Steps_Filt(void)//filter bank
/**---------------------------------------------------------------------
* Function : Pedometer_Filter
* Description : 计步器滤波器,一共有4个频段的低通滤波器。针对x,y,z,mod(x,y,z)
* 的数据进行滤波处理，一共得到16轴的输出数据。
* Date : 2023/02/25
*---------------------------------------------------------------------**/
static void Pedometer_Filter(void)
{
short i, j;
double FiltSum;
for (i = 0; i < AxisNumberFiltOut; i++)
{
for (i = 0; i < FILTER_AXIS_NUM; i++){
DifFiltOutput[i][0] = DifFiltOutput[i][1];
DifFiltOutput[i][1] = DifFiltOutput[i][2];
DifFiltOutput[i][2] = DifFiltOutput[i][3];
DifFiltOutput[i][3] = DifFiltOutput[i][4];
OutputFilt[i][0] = OutputFilt[i][1];
OutputFilt[i][0] = OutputFilt[i][1];
}
for (i = 0; i < AxisNumber; i++)
{
for (i = 0; i < AXIS_NUM; i++){
FiltSum = 0;
/*filterϵ<72><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ5*/
for (j = 0; j < 4; j++)
{
FiltSum += DifFiltCoefficientOne[j] * (stepsFiltAcc[i][15 + j] + stepsFiltAcc[i][23 - j]);
FiltSum += DifFiltCoefficient1[j] * (AccBuffer[i][15 + j] + AccBuffer[i][23 - j]);
}
FiltSum += DifFiltCoefficientOne[4] * stepsFiltAcc[i][19];
FiltSum += DifFiltCoefficient1[4] * AccBuffer[i][19];
DifFiltOutput[i][4] = (FiltSum);
FiltSum = 0;
/*filterϵ<72><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ9*/
for (j = 0; j < 8; j++)
{
FiltSum += DifFiltCoefficientTwo[j] * (stepsFiltAcc[i][11 + j] + stepsFiltAcc[i][27 - j]);
FiltSum += DifFiltCoefficient2[j] * (AccBuffer[i][11 + j] + AccBuffer[i][27 - j]);
}
FiltSum += DifFiltCoefficientTwo[8] * stepsFiltAcc[i][19];
DifFiltOutput[i + AxisNumber][4] = (FiltSum);
FiltSum += DifFiltCoefficient2[8] * AccBuffer[i][19];
DifFiltOutput[i + AXIS_NUM][4] = (FiltSum);
FiltSum = 0;
/*filterϵ<72><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ13*/
for (j = 0; j < 12; j++)
{
FiltSum += DifFiltCoefficientThree[j] * (stepsFiltAcc[i][7 + j] + stepsFiltAcc[i][31 - j]);
for (j = 0; j < 12; j++){
FiltSum += DifFiltCoefficient3[j] * (AccBuffer[i][7 + j] + AccBuffer[i][31 - j]);
}
FiltSum += DifFiltCoefficientThree[12] * stepsFiltAcc[i][19];
DifFiltOutput[i + AxisNumber * 2][4] = (FiltSum);
FiltSum += DifFiltCoefficient3[12] * AccBuffer[i][19];
DifFiltOutput[i + AXIS_NUM * 2][4] = (FiltSum);
FiltSum = 0;
/*filterϵ<72><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ17*/
for (j = 0; j < 16; j++)
{
FiltSum += DifFiltCoefficientFour[j] * (stepsFiltAcc[i][3 + j] + stepsFiltAcc[i][35 - j]);
for (j = 0; j < 16; j++){
FiltSum += DifFiltCoefficient4[j] * (AccBuffer[i][3 + j] + AccBuffer[i][35 - j]);
}
FiltSum += DifFiltCoefficientFour[16] * stepsFiltAcc[i][19];
DifFiltOutput[i + AxisNumber * 3][4] = (FiltSum);
FiltSum += DifFiltCoefficient4[16] * AccBuffer[i][19];
DifFiltOutput[i + AXIS_NUM * 3][4] = (FiltSum);
}
for (i = 0; i < AxisNumberFiltOut; i++)
{
// 更新16个轴计步器的输出结果
for (i = 0; i < FILTER_AXIS_NUM; i++){
OutputFilt[i][1] = DifFiltOutput[i][4];
}
}
/*
StepLcVar <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16*3
StepLcVar[nA][0]<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.<EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
StepLcVar[nA][1]<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>½<EFBFBD><EFBFBD>ı<EFBFBD>־<EFBFBD><EFBFBD>1:<EFBFBD><EFBFBD><EFBFBD><EFBFBD> -1:<EFBFBD>½<EFBFBD> 0:<EFBFBD><EFBFBD>ʼֵ
StepLcVar[nA][2]<EFBFBD>洢<EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>򲨹ȵķ<EFBFBD>ֵ
StepFetureAmp <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16*8 <EFBFBD><EFBFBD><EFBFBD>ڴ洢<EFBFBD><EFBFBD><EFBFBD>ֵ
StepFetureTime <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16*8 <EFBFBD><EFBFBD><EFBFBD>ڴ洢<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
StepReFeature <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ16*1
0 filter one and acc x
1 filter one and acc y
2 filter one and acc z
3 filter one and acc xyz
4 filter two and acc x
5 filter two and acc y
6 filter two and acc z
7 filter two and acc xyz
8 filter three and acc x
9 filter three and acc y
10 filter three and acc z
11 filter three and acc xyz
12 filter four and acc x
13 filter four and acc y
14 filter four and acc z
15 filter four and acc xyz
*/
static void Steps_Feature(void)
{
short nA, nB;
for (nA = 0; nA < AxisNumberFiltOut; nA++)
for (nA = 0; nA < FILTER_AXIS_NUM; nA++)
{
/*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.ÿ<>μӣ<CEBC>*/
StepLcVar[nA][0] = StepLcVar[nA][0] + 1;
/*<2A>Ҳ<EFBFBD><D2B2>ȣ<EFBFBD><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(StepLcVar[nA][1] <1)<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(OutputFilt[nA][1] > OutputFilt[nA][0])<29><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ұߵ<D2B1><DFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> StepLcVar[nA][1] = 1*/
if ((StepLcVar[nA][1] < 1) && ((OutputFilt[nA][1] > OutputFilt[nA][0]) || ((OutputFilt[nA][1] == OutputFilt[nA][0]) && (StepLcVar[nA][1] == 1))))
if ((StepLcVar[nA][1] < 1) && ((OutputFilt[nA][1] > OutputFilt[nA][0]) ||
((OutputFilt[nA][1] == OutputFilt[nA][0]) && (StepLcVar[nA][1] == 1))))
{
for (nB = 0; nB < 7; nB++)
{
StepFetureAmp[nA][nB] = StepFetureAmp[nA][nB + 1];
}
/*<2A>洢<EFBFBD><E6B4A2><EFBFBD>ֵ=<3D><>һ<EFBFBD>β<EFBFBD><CEB2><EFBFBD>ֵ-<2D><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ֵ*/
StepFetureAmp[nA][7] = StepLcVar[nA][2] - OutputFilt[nA][0];
/*<2A>洢<EFBFBD><E6B4A2>ǰ<EFBFBD><C7B0><EFBFBD>ȵķ<C8B5>ֵ*/
StepLcVar[nA][2] = OutputFilt[nA][0];
for (nB = 0; nB < 7; nB++)
{
StepFetureTime[nA][nB] = StepFetureTime[nA][nB + 1];
}
/*<2A>洢<EFBFBD><E6B4A2><EFBFBD>ļ<EFBFBD><C4BC>ʱ<EFBFBD><CAB1>*/
StepFetureTime[nA][7] = (unsigned short)(StepLcVar[nA][0]);
/*<2A>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD>Ⱥ󣬼<C8BA><F3A3ACBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
StepLcVar[nA][0] = 0;
/*<2A>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD>Ⱥ󣬼<C8BA>¼<EFBFBD><C2BC><EFBFBD><EFBFBD>״̬*/
StepLcVar[nA][1] = 1;
StepReFeature[nA] = 1;
}
@ -451,19 +391,14 @@ static void Steps_Feature(void)
{
StepFetureAmp[nA][nB] = StepFetureAmp[nA][nB + 1];
}
/*<2A>洢<EFBFBD><E6B4A2><EFBFBD>ֵ=<3D><>һ<EFBFBD>β<EFBFBD><CEB2><EFBFBD>ֵ-<2D><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ֵ*/
StepFetureAmp[nA][7] = OutputFilt[nA][0] - StepLcVar[nA][2];
/*<2A>洢<EFBFBD><E6B4A2>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ķ<EFBFBD>ֵ*/
StepLcVar[nA][2] = OutputFilt[nA][0];
for (nB = 0; nB < 7; nB++)
{
StepFetureTime[nA][nB] = StepFetureTime[nA][nB + 1];
}
/*<2A>洢<EFBFBD><E6B4A2><EFBFBD>ļ<EFBFBD><C4BC>ʱ<EFBFBD><CAB1>*/
StepFetureTime[nA][7] = (unsigned short)(StepLcVar[nA][0]);
/*<2A>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD><EFBFBD>󣬼<EFBFBD><F3A3ACBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
StepLcVar[nA][0] = 0;
/*<2A>ҵ<EFBFBD><D2B5><EFBFBD><EFBFBD>Ⱥ󣬼<C8BA>¼<EFBFBD>Ͻ<EFBFBD>״̬*/
StepLcVar[nA][1] = -1;
StepReFeature[nA] = 1;
}
@ -477,10 +412,10 @@ static void Steps_Rcording(unsigned long long timeStamp)
double StepFetureAmp12, StepFetureAmp34, StepFetureAmp56, StepFetureAmp78, StepFetureAmp14, StepFetureAmp58, StepFetureAmp18;
unsigned short StepFetureTime12, StepFetureTime34, StepFetureTime56, StepFetureTime78, StepFetureTime14, StepFetureTime58, StepFetureTime38, StepFetureTime18;
int j;
for (nC = AxisNumber; nC < AxisNumberStepSaveState; nC++)
for (nC = AXIS_NUM; nC < AxisNumberStepSaveState; nC++)
{
if ((StepRecordeStateTime[nC][4] > 5) && ((nD == 0) || (nD > StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4])) && (nC != AxisNumberFiltOut))
if ((StepRecordeStateTime[nC][4] > 5) && ((nD == 0) || (nD > StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4])) && (nC != FILTER_AXIS_NUM))
{
nD = StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4];
}
@ -488,23 +423,23 @@ static void Steps_Rcording(unsigned long long timeStamp)
if (nD > 0)
{
for (nC = 0; nC < AxisNumber - 1; nC++) // nC<AxisNumber;
for (nC = 0; nC < AXIS_NUM - 1; nC++) // nC<AxisNumber;
{
if ((StepRecordeStateTime[nC][4] > 5) && (nD > StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4]))
{
nD = StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4];
}
}
nC = AxisNumberFiltOut;
nC = FILTER_AXIS_NUM;
if ((StepRecordeStateTime[nC][4] > 5) && (nD > StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4]))
{
nD = StepRecordeStateTime[nC][5] / StepRecordeStateTime[nC][4];
}
}
Differ_Sum = 0;
for (j = 0; j < AccelerationNumber; j++)
for (j = 0; j < ACC_BUFF_NUM; j++)
{
Differ_Sum = Differ_Sum + fabs(stepsFiltAcc[3][j + 1] - stepsFiltAcc[3][j]);
Differ_Sum = Differ_Sum + fabs(AccBuffer[3][j + 1] - AccBuffer[3][j]);
}
nC = 3; //20180917,nC=4 to 3;
@ -522,7 +457,7 @@ static void Steps_Rcording(unsigned long long timeStamp)
}
}
for (nA = 0; nA < AxisNumberFiltOut; nA++)
for (nA = 0; nA < FILTER_AXIS_NUM; nA++)
{
if (StepReFeature[nA] == 1)
{
@ -550,17 +485,17 @@ static void Steps_Rcording(unsigned long long timeStamp)
StepRecordeStateTime[nA][3] = StepRecordeStateTime[nA][3] + StepFetureTime[nA][7];
/*(nA % 4 == 3)Ϊ<>ϼ<EFBFBD><CFBC>ٶȾ<D9B6><C8BE>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ*/
if ((nA % 4 == 3) && (StepRecordeState[AxisNumberFiltOut + nA / 4][2] < 30))
if ((nA % 4 == 3) && (StepRecordeState[FILTER_AXIS_NUM + nA / 4][2] < 30))
{
StepRecordeState[AxisNumberFiltOut + nA / 4][2] = StepRecordeState[AxisNumberFiltOut + nA / 4][2] + 0.51;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][2] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][2] + StepFetureTime[nA][7];
StepRecordeState[AxisNumberFiltOut + nA / 4][3] = StepRecordeState[AxisNumberFiltOut + nA / 4][3] + 0.51;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] + StepFetureTime[nA][7];
StepRecordeState[FILTER_AXIS_NUM + nA / 4][2] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][2] + 0.51;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][2] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][2] + StepFetureTime[nA][7];
StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] + 0.51;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3] + StepFetureTime[nA][7];
}
}
/*0.1538 0.1445 0.1352 0.1260 0.1167 0.1075 0.0982 0.0890 0.0798 0.0705 0.0612 0.0520 0.0427 0.0335 0.0243 0.0150*/
thred = 0.015 + ((AxisNumberFiltOut - nA - 1) / AxisNumber) * 0.037;//dynamic threshold
thred = 0.015 + ((FILTER_AXIS_NUM - nA - 1) / AXIS_NUM) * 0.037;//dynamic threshold
if (StepRecordeStateTime[nA][6] < 60) //25
{
@ -576,15 +511,15 @@ static void Steps_Rcording(unsigned long long timeStamp)
/*(nA % 4 == 3)Ϊ<>ϼ<EFBFBD><CFBC>ٶȾ<D9B6><C8BE>˲<EFBFBD><CBB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ*/
if (nA % 4 == 3)
{
if (StepRecordeStateTime[AxisNumberFiltOut + nA / 4][6] < 25)
if (StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][6] < 25)
{
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][6]++;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][6]++;
}
else
{
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] = 0;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = 0;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7] = 0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] = 0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5] = 0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][7] = 0;
}
if ((fabs(StepFetureAmp[nA][2] - StepFetureAmp[nA][3]) / (StepFetureAmp34) < 0.15l)
&& (fabs(StepFetureAmp[nA][3] - StepFetureAmp[nA][4]) / (StepFetureAmp[nA][3] + StepFetureAmp[nA][4]) < 0.15l)
@ -600,15 +535,15 @@ static void Steps_Rcording(unsigned long long timeStamp)
&& (StepFetureTime58 > StepMinTime)
&& (StepFetureTime58 < StepMaxTime))
{
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][8] = SampleFrequency * 3.0 / StepFetureTime38;
timeStampSave[AxisNumberFiltOut + nA / 4][1] = timeStamp;
if (StepRecordeState[AxisNumberFiltOut + nA / 4][0] == 0)
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][8] = PEDOMETER_FREQ * 3.0 / StepFetureTime38;
timeStampSave[FILTER_AXIS_NUM + nA / 4][1] = timeStamp;
if (StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] == 0)
{
StepRecordeState[AxisNumberFiltOut + nA / 4][0] = StepRecordeState[AxisNumberFiltOut + nA / 4][0] + 3;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] + StepFetureTime38;
StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + 3;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] + StepFetureTime38;
timeStampSave[AxisNumberFiltOut + nA / 4][0] = timeStamp - (unsigned long)(StepFetureTime38 * 1000 / SampleFrequency);
StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] + 3;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] + StepFetureTime38;
StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] + 3;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][1] + StepFetureTime38;
timeStampSave[FILTER_AXIS_NUM + nA / 4][0] = timeStamp - (unsigned long)(StepFetureTime38 * 1000 / PEDOMETER_FREQ);
}
else
{
@ -618,78 +553,78 @@ static void Steps_Rcording(unsigned long long timeStamp)
&& ((ABS_INT(StepFetureTime[nA][5] - StepFetureTime[nA][7]) < 0.2 * (StepFetureTime[nA][5] + StepFetureTime[nA][7])) || (ABS_INT(StepFetureTime[nA][5] - StepFetureTime[nA][7]) < 5)))
{
if ((StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7] > 0) && (fabs(StepFetureTime38 / 3.0 - StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7]) > 0.2 * StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7]))
if ((StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][7] > 0) && (fabs(StepFetureTime38 / 3.0 - StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][7]) > 0.2 * StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][7]))
{
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] = 0;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = 0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] = 0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5] = 0;
}
if (StepRecordeStateTime[AxisNumberFiltOut + nA / 4][6] >= 6)
if (StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][6] >= 6)
{
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] + 3;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] + StepFetureTime38;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][6] = 0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] + 3;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5] + StepFetureTime38;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][6] = 0;
}
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7] = StepFetureTime38 / 3.0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][7] = StepFetureTime38 / 3.0;
}
if (StepRecordeState[AxisNumberFiltOut + nA / 4][3] <= 3)
if (StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] <= 3)
{
StepRecordeState[AxisNumberFiltOut + nA / 4][0] = StepRecordeState[AxisNumberFiltOut + nA / 4][0] + StepRecordeState[AxisNumberFiltOut + nA / 4][3];
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] + StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3];
StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] + StepRecordeState[FILTER_AXIS_NUM + nA / 4][3];
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3];
}
else
{
StepRecordeState[AxisNumberFiltOut + nA / 4][0] = StepRecordeState[AxisNumberFiltOut + nA / 4][0] + 3;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] + StepFetureTime38;
StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] + 3;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] + StepFetureTime38;
}
if (StepRecordeState[AxisNumberFiltOut + nA / 4][3] <= 3)
if (StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] <= 3)
{
StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + StepRecordeState[AxisNumberFiltOut + nA / 4][3];
StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] + StepRecordeState[FILTER_AXIS_NUM + nA / 4][3];
}
else if ((StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] - StepFetureTime38 > StepMaxTime)
&& (StepRecordeState[AxisNumberFiltOut + nA / 4][3] > 11))
else if ((StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3] - StepFetureTime38 > StepMaxTime)
&& (StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] > 11))
{
StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + 3;
StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] + 3;
}
else
{
if (StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] > 0)
if (StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] > 0)
{
nE = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] * StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] / StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5];
nE = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3] * StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] / StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5];
}
if ((StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] > 0) && (nE < StepRecordeState[AxisNumberFiltOut + nA / 4][3]))
if ((StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] > 0) && (nE < StepRecordeState[FILTER_AXIS_NUM + nA / 4][3]))
{
if (nE > 4)
{
StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + nE;
StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] + nE;
}
else
{
StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + 3;
StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] + 3;
}
}
else
{
if (StepRecordeState[AxisNumberFiltOut + nA / 4][3] > 4)
if (StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] > 4)
{
StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + StepRecordeState[AxisNumberFiltOut + nA / 4][3];
StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] + StepRecordeState[FILTER_AXIS_NUM + nA / 4][3];
}
else
{
StepRecordeState[AxisNumberFiltOut + nA / 4][1] = StepRecordeState[AxisNumberFiltOut + nA / 4][1] + 3;
StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] + 3;
}
}
}
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] = StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] + StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3];
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][1] = StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][1] + StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3];
}
StepRecordeState[AxisNumberFiltOut + nA / 4][2] = 0;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][2] = 0;
StepRecordeState[AxisNumberFiltOut + nA / 4][3] = 0;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] = 0;
StepRecordeState[FILTER_AXIS_NUM + nA / 4][2] = 0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][2] = 0;
StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] = 0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3] = 0;
}
else if ((StepRecordeState[AxisNumberFiltOut + nA / 4][0] > 0) && ((StepRecordeStateTime[AxisNumberFiltOut + nA / 4][2] >= StepMaxTime * 2) || (StepFetureAmp18 < 4 * thred) || (StateDetectLcvar[2] > SampleFrequency)))
else if ((StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] > 0) && ((StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][2] >= StepMaxTime * 2) || (StepFetureAmp18 < 4 * thred) || (StateDetectLcVar[2] > PEDOMETER_FREQ)))
{
nB = AxisNumberStepSaveState - 1;
if (nD > 0)
@ -704,15 +639,15 @@ static void Steps_Rcording(unsigned long long timeStamp)
}
else
{
for (nC = (AxisNumberStepSaveState - 1); nC >= AxisNumber; nC--)
for (nC = (AxisNumberStepSaveState - 1); nC >= AXIS_NUM; nC--)
{
if ((StepRecordeStateTime[nB][0] < StepRecordeStateTime[nC][0]) && (nC != AxisNumberFiltOut))
if ((StepRecordeStateTime[nB][0] < StepRecordeStateTime[nC][0]) && (nC != FILTER_AXIS_NUM))
{
nB = nC;
}
}
}
if ((nB == AxisNumberFiltOut + nA / 4) && (StepRecordeState[nB][0] >= 8))
if ((nB == FILTER_AXIS_NUM + nA / 4) && (StepRecordeState[nB][0] >= 8))
{
if (Steps.walkRealTime > StepRecordeState[nB][1])
{
@ -732,7 +667,7 @@ static void Steps_Rcording(unsigned long long timeStamp)
//#endif
for (nC = 0; nC < AxisNumberStepSaveState; nC++)
{
if ((StepFetureAmp18 < 4 * thred) || (StateDetectLcvar[2] > SampleFrequency))
if ((StepFetureAmp18 < 4 * thred) || (StateDetectLcVar[2] > PEDOMETER_FREQ))
{
StepRecordeState[nC][1] = 0;
StepRecordeStateTime[nC][1] = 0;
@ -773,17 +708,17 @@ static void Steps_Rcording(unsigned long long timeStamp)
}
}
}
StepRecordeState[AxisNumberFiltOut + nA / 4][0] = 0;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][0] = 0;
StepRecordeState[AxisNumberFiltOut + nA / 4][1] = 0;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][1] = 0;
StepRecordeState[AxisNumberFiltOut + nA / 4][3] = 0;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][3] = 0;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][4] = 0;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][5] = 0;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][7] = 0;
StepRecordeStateTime[AxisNumberFiltOut + nA / 4][8] = 0;
timeStampSave[AxisNumberFiltOut + nA / 4][0] = 0;
StepRecordeState[FILTER_AXIS_NUM + nA / 4][0] = 0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][0] = 0;
StepRecordeState[FILTER_AXIS_NUM + nA / 4][1] = 0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][1] = 0;
StepRecordeState[FILTER_AXIS_NUM + nA / 4][3] = 0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][3] = 0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][4] = 0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][5] = 0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][7] = 0;
StepRecordeStateTime[FILTER_AXIS_NUM + nA / 4][8] = 0;
timeStampSave[FILTER_AXIS_NUM + nA / 4][0] = 0;
}
}
@ -802,11 +737,11 @@ static void Steps_Rcording(unsigned long long timeStamp)
if ((StepFetureTime58 >= StepMaxTime)
|| ((nD != 0) && (nD < 0.7l * StepFetureTime18 / 4.0l)))
{
StepRecordeStateTime[nA][8] = SampleFrequency * 8.0 / StepFetureTime18;
StepRecordeStateTime[nA][8] = PEDOMETER_FREQ * 8.0 / StepFetureTime18;
}
else
{
StepRecordeStateTime[nA][8] = SampleFrequency * 4.0 / StepFetureTime18;
StepRecordeStateTime[nA][8] = PEDOMETER_FREQ * 4.0 / StepFetureTime18;
}
timeStampSave[nA][1] = timeStamp;
if (StepRecordeState[nA][0] == 0)
@ -823,7 +758,7 @@ static void Steps_Rcording(unsigned long long timeStamp)
StepRecordeState[nA][1] = StepRecordeState[nA][1] + 4;
}
StepRecordeStateTime[nA][1] = StepRecordeStateTime[nA][1] + StepFetureTime18;
timeStampSave[nA][0] = timeStamp - (unsigned long)(StepFetureTime18 * 1000 / SampleFrequency);
timeStampSave[nA][0] = timeStamp - (unsigned long)(StepFetureTime18 * 1000 / PEDOMETER_FREQ);
}
else
{
@ -951,7 +886,7 @@ static void Steps_Rcording(unsigned long long timeStamp)
StepRecordeState[nA][3] = 0;
StepRecordeStateTime[nA][3] = 0;
}
else if ((StepRecordeState[nA][0] > 0) && ((StepRecordeStateTime[nA][2] >= StepMaxTime * 2) || (StepFetureAmp18 < 4 * thred) || (StateDetectLcvar[2] > SampleFrequency)))
else if ((StepRecordeState[nA][0] > 0) && ((StepRecordeStateTime[nA][2] >= StepMaxTime * 2) || (StepFetureAmp18 < 4 * thred) || (StateDetectLcVar[2] > PEDOMETER_FREQ)))
{
nB = AxisNumberStepSaveState - 1;
if (nD > 0)
@ -966,9 +901,9 @@ static void Steps_Rcording(unsigned long long timeStamp)
}
else
{
for (nC = (AxisNumberStepSaveState - 1); nC >= AxisNumber; nC--)
for (nC = (AxisNumberStepSaveState - 1); nC >= AXIS_NUM; nC--)
{
if ((StepRecordeStateTime[nB][0] < StepRecordeStateTime[nC][0]) && (nC != AxisNumberFiltOut))
if ((StepRecordeStateTime[nB][0] < StepRecordeStateTime[nC][0]) && (nC != FILTER_AXIS_NUM))
{
nB = nC;
}
@ -986,14 +921,10 @@ static void Steps_Rcording(unsigned long long timeStamp)
}
Steps.walkRealTime = 0;
ReSteps.walkSteps = (unsigned long)(Steps.walk);
//#if defined(PLATFORM_PC_WINDOWS) || defined(PLATFORM_PC_LINUX)
// steps_back(ReSteps);
//#elif defined(PLATFORM_ANDROID_QCOM_AP) || defined(PLATFORM_ANDROID_QCOM_ADSP)
// AutoDisplayStep_Event_Handler(ReSteps);
//#endif
for (nC = 0; nC < AxisNumberStepSaveState; nC++)
{
if ((StepFetureAmp18 < 4 * thred) || (StateDetectLcvar[2] > SampleFrequency))
if ((StepFetureAmp18 < 4 * thred) || (StateDetectLcVar[2] > PEDOMETER_FREQ))
{
StepRecordeState[nC][1] = 0;
StepRecordeStateTime[nC][1] = 0;
@ -1204,119 +1135,114 @@ static void State_Detect(unsigned long long timeStamp)
double SumAccYZ = 0;
double AccX = 0;
double AccYZ = 0;
/*double Mean_x = 0, Mean_y = 0, Mean_z = 0;*/
/*double DeDC_x = 0, DeDC_y = 0, DeDC_z = 0, DeDC_xyz = 0;*/
#if defined(PLATFORM_ANDROID_QCOM_SLPI845)
(void)timeStamp;
#endif
if (StateDetectLcvar[0] < StepMinTime)
if (StateDetectLcVar[0] < StepMinTime)
{
StateDetectLcvar[0] = StateDetectLcvar[0] + 1;
if (StateDetectLcvar[0] < 0.5l)
StateDetectLcVar[0] = StateDetectLcVar[0] + 1;
if (StateDetectLcVar[0] < 0.5l)
{
SumAcc = -1;
}
else
{
}else{
SumAcc = SumAcc - 1;
}
}
else
{
SumAcc = 0;
for (nA = 0; nA < (AccelerationNumber - 1); nA++)
for (nA = 0; nA < (ACC_BUFF_NUM - 1); nA++)
{
SumAcc += fabs(stepsFiltAcc[3][nA] - stepsFiltAcc[3][nA + 1]);
SumAccX += fabs(stepsFiltAcc[0][nA] - stepsFiltAcc[0][nA + 1]);
SumAccYZ += fabs(sqrt(stepsFiltAcc[1][nA] * stepsFiltAcc[1][nA] + stepsFiltAcc[2][nA] * stepsFiltAcc[2][nA])
- sqrt(stepsFiltAcc[1][nA + 1] * stepsFiltAcc[1][nA + 1] + stepsFiltAcc[2][nA + 1] * stepsFiltAcc[2][nA + 1]));
SumAcc += fabs(AccBuffer[3][nA] - AccBuffer[3][nA + 1]);
SumAccX += fabs(AccBuffer[0][nA] - AccBuffer[0][nA + 1]);
SumAccYZ += fabs(sqrt(AccBuffer[1][nA] * AccBuffer[1][nA] + AccBuffer[2][nA] * AccBuffer[2][nA])
- sqrt(AccBuffer[1][nA + 1] * AccBuffer[1][nA + 1] + AccBuffer[2][nA + 1] * AccBuffer[2][nA + 1]));
}
SumAcc /= (AccelerationNumber - 1);
SumAccX /= (AccelerationNumber - 1);
SumAccYZ /= (AccelerationNumber - 1);
for (nA = 0; nA < AccelerationNumber; nA++)
{
AccX += fabs(stepsFiltAcc[0][nA]);
}
AccX = AccX / AccelerationNumber;
SumAcc /= (ACC_BUFF_NUM - 1);
SumAccX /= (ACC_BUFF_NUM - 1);
SumAccYZ /= (ACC_BUFF_NUM - 1);
for (nA = 0; nA < AccelerationNumber; nA++)
{
AccYZ += sqrt(stepsFiltAcc[1][nA] * stepsFiltAcc[1][nA] + stepsFiltAcc[2][nA] * stepsFiltAcc[2][nA]);
for (nA = 0; nA < ACC_BUFF_NUM; nA++){
AccX += fabs(AccBuffer[0][nA]);
}
AccYZ /= AccelerationNumber;
AccX = AccX / ACC_BUFF_NUM;
for (nA = 0; nA < ACC_BUFF_NUM; nA++)
{
AccYZ += sqrt(AccBuffer[1][nA] * AccBuffer[1][nA] + AccBuffer[2][nA] * AccBuffer[2][nA]);
}
AccYZ /= ACC_BUFF_NUM;
if (SumAcc > 3 || SumAcc < 0.002l)
{
if (StateDetectLcvar[2] < SampleFrequencyFour)
if (StateDetectLcVar[2] < SampleFrequencyFour)
{
StateDetectLcvar[2] = StateDetectLcvar[2] + StepMinTime;
StateDetectLcVar[2] = StateDetectLcVar[2] + StepMinTime;
}
StateDetectLcvar[3] = 0;
StateDetectLcVar[3] = 0;
}
else
{
if (StateDetectLcvar[3] < SampleFrequencyFour)
if (StateDetectLcVar[3] < SampleFrequencyFour)
{
StateDetectLcvar[3] = StateDetectLcvar[3] + StepMinTime;
StateDetectLcVar[3] = StateDetectLcVar[3] + StepMinTime;
}
StateDetectLcvar[2] = 0;
StateDetectLcVar[2] = 0;
}
if (StateDetectLcvar[2] > SampleFrequencydouble)
if (StateDetectLcVar[2] > SampleFrequencydouble)
{
StateDetectLcvar[4] = 1;
StateDetectLcVar[4] = 1;
}
else if (StateDetectLcvar[2] > SampleFrequencyOPF)
else if (StateDetectLcVar[2] > SampleFrequencyOPF)
{
STEPLIB_MEMSET(StepReFeature, 1, sizeof(StepReFeature));
}
if (StateDetectLcvar[3] > StepMinTime)
if (StateDetectLcVar[3] > StepMinTime)
{
StateDetectLcvar[4] = 0;
StateDetectLcVar[4] = 0;
}
if (((AccX * 1.3l > AccYZ) || (SumAccX * 1.3l > SumAccYZ)) && (SumAcc < 0.8l))
{
if (StateDetectLcvar[1] > 0)
if (StateDetectLcVar[1] > 0)
{
StateDetectLcvar[1] = StateDetectLcvar[1] - 1;
StateDetectLcVar[1] = StateDetectLcVar[1] - 1;
}else{
StateDetectLcvar[1] = 0;
StateDetectLcVar[1] = 0;
}
}
else
{
if ((StateDetectLcvar[1] > 4) || (SumAcc > 0.6l))
if ((StateDetectLcVar[1] > 4) || (SumAcc > 0.6l))
{
StateDetectLcvar[1] = 8;//10
StateDetectLcVar[1] = 8;//10
}
else
{
if ((AccX < AccYZ) && (SumAcc > 0.25l))
{
if (StateDetectLcvar[1] < 8)//10
if (StateDetectLcVar[1] < 8)//10
{
StateDetectLcvar[1] = StateDetectLcvar[1] + 1;
StateDetectLcVar[1] = StateDetectLcVar[1] + 1;
}
}
else
{
StateDetectLcvar[1] = 0;
StateDetectLcVar[1] = 0;
}
}
}
if (StateDetectLcvar[1] > 4){
if (StateDetectLcVar[1] > 4){
stateLcVar[0] = 2;
}else{
stateLcVar[0] = walk;
}
StateDetectLcvar[0] = 0;
StateDetectLcVar[0] = 0;
}
}
void Steps_State_Detect(enumState* state){
void Pedometer_GetState(StepState_t* state){
*state = stateLcVar[0];
}

31
1.Software/PDR 1.06/gnssIns.sln Normal file
View File

@ -0,0 +1,31 @@

Microsoft Visual Studio Solution File, Format Version 12.00
# Visual Studio Version 16
VisualStudioVersion = 16.0.30804.86
MinimumVisualStudioVersion = 10.0.40219.1
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "PDR", "project\PDR.vcxproj", "{28D32532-309F-40EA-9E4A-2D162CC434D2}"
EndProject
Global
GlobalSection(SolutionConfigurationPlatforms) = preSolution
Debug|x64 = Debug|x64
Debug|x86 = Debug|x86
Release|x64 = Release|x64
Release|x86 = Release|x86
EndGlobalSection
GlobalSection(ProjectConfigurationPlatforms) = postSolution
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Debug|x64.ActiveCfg = Debug|x64
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Debug|x64.Build.0 = Debug|x64
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Debug|x86.ActiveCfg = Debug|Win32
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Debug|x86.Build.0 = Debug|Win32
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Release|x64.ActiveCfg = Release|x64
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Release|x64.Build.0 = Release|x64
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Release|x86.ActiveCfg = Release|Win32
{28D32532-309F-40EA-9E4A-2D162CC434D2}.Release|x86.Build.0 = Release|Win32
EndGlobalSection
GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE
EndGlobalSection
GlobalSection(ExtensibilityGlobals) = postSolution
SolutionGuid = {B6378B77-5AB0-4A12-ACE8-2B45F292D7EE}
EndGlobalSection
EndGlobal

48
1.Software/PDR 1.06/include/AHRS.h Normal file
View File

@ -0,0 +1,48 @@
#ifndef _PDR_AHRS_H_
#define _PDR_AHRS_H_
#ifdef __cplusplus
extern "C" {
#endif
/* Header File Including ------------------------------------------------------------------------ */
#include "pdr_sensor.h"
/* Macro Declaration ---------------------------------------------------------------------------- */
#define AHRS_SAMPLE_FREQ 100 // ARHS算法的传感器采样频率
#define IMU_SAMPLING_FREQUENCY 100
#define AHRS_KP 0.500
#define AHRS_TYPE_GYRO 4
#define AHRS_TYPE_ACCE 1
#define AHRS_TYPE_MAGN 2
#define AHRS_STATUS_RESET_PHASE_0 0x80
#define AHRS_STATUS_RESET_PHASE_1 0x40
#define AHRS_STATUS_RESET (AHRS_STATUS_RESET_PHASE_0|AHRS_STATUS_RESET_PHASE_1)
#define AHRS_STATUS_STABLE 0x20
/* Struct Declaration --------------------------------------------------------------------------- */
/* Function Declaration ------------------------------------------------------------------------- */
void AHRS_Init(void);
void ResetARHS(void);
/**---------------------------------------------------------------------
* Function : fv3Norm
* Description : 三维浮点数向量归一化
* Date : 2021/01/26 yuanlin@vivocom &&
*
*
*---------------------------------------------------------------------**/
void fv3Norm(float* vx, float* vy, float* vz);
int UpdateAHRS(IMU_t* imu);
#ifdef __cplusplus
}
#endif
#endif // for __AHRS_H

18
1.Software/PDR 1.06/include/Fft.h Normal file
View File

@ -0,0 +1,18 @@
#ifndef __PDR_FFT_H__
#define __PDR_FFT_H__
/* Header File Including ------------------------------------------------------------------------ */
#include <stdint.h>
#include <math.h>
#include "Complex.h"
/* Macro Declaration ---------------------------------------------------------------------------- */
#define M_PI 3.14159265358979323846 /* pi */
/* Structure Declaration ------------------------------------------------------------------------ */
/* Function Declaration ------------------------------------------------------------------------- */
extern int FFT_Dft(Complex_t *fft, float *signal, int length);
extern int FFT_Fft(Complex_t *fft, float *signal, int length);
#endif

52
1.Software/PDR 1.06/include/Filter.h Normal file
View File

@ -0,0 +1,52 @@
/******************** (C) COPYRIGHT 2022 Geek************************************
* File Name : Filter.c
* Current Version : V2.0
* Author : logzhan
* Date of Issued : 2022.10.15
* Comments : PDR滤波器支持库
********************************************************************************/
/* Header File Including ------------------------------------------------------*/
#ifndef __FILTER_H__
#define __FILTER_H__
/* Macro Declaration ----------------------------------------------------------*/
#define FILTER_MAX_LENGTH 71
/* Structure Declaration ------------------------------------------------------*/
typedef struct Filter_t { // iir or fir filter
int reset; // reset flag
int order; // coefficient array length, not filter order
double H0; // dc signal gain
double b[FILTER_MAX_LENGTH]; // b coefficient
double a[FILTER_MAX_LENGTH]; // a coefficient
double yout[FILTER_MAX_LENGTH]; // yout buffer
double xin[FILTER_MAX_LENGTH]; // xin buffer
} Filter_t;
/* Function Declaration -------------------------------------------------------*/
/**---------------------------------------------------------------------
* Function : FilterReset
* Description : 重置滤波器
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void FilterReset(Filter_t *f);
/**---------------------------------------------------------------------
* Function : FilterSetCoef
* Description : 滤波器设置参数
* Input : order coefficient array length, not filter order
* *b b coefficient
* *a a coefficient
* H0 dc signal gain
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void FilterSetCoef(Filter_t *f, int order, double *b, double *a, double H0);
/**---------------------------------------------------------------------
* Function : FilterFilter
* Description : 滤波器滤波计算
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
double FilterFilter(Filter_t *f, double x);
#endif

54
1.Software/PDR 1.06/include/Kalman.h Normal file
View File

@ -0,0 +1,54 @@
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : pdr_kalman.h
* Department : Sensor Algorithm Team
* Current Version : V2.0(compare QCOM SAP 5.0)
* Author : logzhan
* Date of Issued : 2020.7.3
* Comments : PDR 卡尔曼滤波器头文件函数声明以及相关结构体定义
********************************************************************************/
#ifndef _PDR_KALMAN_H_
#define _PDR_KALMAN_H_
#ifdef __cplusplus
extern "C" {
#endif
/**----------------------------------------------------------------------
* Function : EKF_Init
* Description : 初始化卡尔曼滤波器相关
* Date : 2022/09/19
*---------------------------------------------------------------------**/
void EKF_Init(void);
/**----------------------------------------------------------------------
* Function : EKFStatePredict
* Description : pdr卡尔曼滤波器的状态预测方程
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
void EKFStatePredict(EKFPara_t* kf, double step_length, PDR_t* g_pdr, int step);
/**----------------------------------------------------------------------
* Function : EKFCalQRMatrix
* Description : 计算卡尔曼滤波噪声矩阵
* scane_type, 用户所处场景（开阔和非开阔）
* nmea_data，NMEA数据结构体
* g_pdr，PDR结构体
* sys，运动幅度数据结构体
* kf，EKF数据结构体
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
void EKFCalQRMatrix(Nmea_t* nmea_data, PDR_t* g_pdr, Classifer_t* sys, EKFPara_t* kf);
/**----------------------------------------------------------------------
* Function : pdr_ekfStateUpdate
* Description : pdr卡尔曼滤波器的状态更新方程
* Date : 2020/7/22 logzhan
*---------------------------------------------------------------------**/
void EKFStateUpdate(EKFPara_t* kf, GNSS_t* pgnss, Classifer_t* sys, PDR_t* g_pdr,
int scene_type);
#ifdef __cplusplus
}
#endif
#endif

66
1.Software/PDR 1.06/include/LinearFit.h Normal file
View File

@ -0,0 +1,66 @@
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : pdr_linearFit.h
* Department : Sensor Algorithm Team
* Current Version : V2.0(compare QCOM SAP 5.0)
* Author :
logzhan
* Date of Issued : 2020.7.4
* Comments : PDR 线性拟合相关函数支持
********************************************************************************/
#ifdef __cplusplus
extern "C" {
#endif
#ifndef _PDR_LINEAR_FIT_H_
#define _PDR_LINEAR_FIT_H_
/**----------------------------------------------------------------------
* Function : pdr_linearLeastSquaresFitting
* Description : 线性最小二乘拟合 Y = c0 + c1 x
目标函数为sum| nDataY - (a*nDataX+b)|^2最小
* Date : 2020/7/4 logzhan
*---------------------------------------------------------------------**/
int linearLeastSquaresFit(const double *nDataX, const double *nDataY,
const int nLength, double *a, double *b);
/**************************************************************************
* Description : 二维坐标系点的线性拟合，ax+by+c=0, 目标函数为点到拟合直线的距离
平方和最小
* Input : x， y，二维坐标X和Y
num, 二维点个数
* Output : para， 拟合直线参数, para[0]:a, para[0]:b, para[0]:c
**************************************************************************/
int leastDistanceLinearFit(double x[], double y[], int num, double para[]);
/**----------------------------------------------------------------------
* Function : gsl_fit_linear
* Description : Fit the data (x_i, y_i) to the linear relationship
Y = c0 + c1 x
returning,
c0, c1 -- coefficients
cov00, cov01, cov11 -- variance-covariance matrix of
c0 and c1,
sumsq -- sum of squares of residuals
This fit can be used in the case where the errors for
the data are uknown, but assumed equal for all points.
The resulting variance-covariance matrix estimates the
error in the coefficientsfrom the observed variance of
the points around the best fit line.
* Date :
*---------------------------------------------------------------------**/
static int gslFitLinear(const double* x, const size_t xstride,
const double* y, const size_t ystride,
const size_t n,
double* c0, double* c1,
double* cov_00, double* cov_01, double* cov_11, double* sumsq);
#endif // !_LINEAR_FITTING_H_
#ifdef __cplusplus
}
#endif

139
1.Software/PDR 1.06/include/Location.h Normal file
View File

@ -0,0 +1,139 @@
#ifndef __PDR_LOCATION_H__
#define __PDR_LOCATION_H__
#ifdef __cplusplus
extern "C" {
#endif
#include "PDRBase.h"
#define HIST_GPS_NUM 8
#define ACCURACY_THRES 0.6f // 精度阈值
#define YAW_THRES 10.0f // Yaw角范围阈值
/**---------------------------------------------------------------------
* Function : NavSys_Init
* Description : PDR导航系统初始化
* Date : 2022/9/16
*---------------------------------------------------------------------**/
void PDR_Init(void);
void PDRInfoInit(void);
/**---------------------------------------------------------------------
* Function : InsLocation
* Description : PDR 惯性导航定位
* Date : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
int InsLocationUpdate(IMU_t *ss_data, EKFPara_t *kf);
/**----------------------------------------------------------------------
* Function : pdr_noGpsPredict
* Description : 在没有gps信息时预测GPS位置，最多预测10个点
* Date : 2020/07/08 logzhan
*---------------------------------------------------------------------**/
void NoGnssInfoPredict(EKFPara_t* kf, LctFs_t* result, PDR_t* g_pdr);
/**----------------------------------------------------------------------
* Function : Nmea2Gnss
* Description : nmea数据结构转gnss数据
* Date : 2020/07/08 logzhan
*---------------------------------------------------------------------**/
void Nmea2Gnss(Nmea_t* nmea_data, GNSS_t* pgnss);
/**----------------------------------------------------------------------
* Function : pdr_detectFixMode
* Description : 检测当前PDR处于的模式，如果是车载和静止模式,根据情况选择输出
* 原始GPS或者不输出
* Date : 2020/07/08 logzhan
* 2020/02/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高
* 代码的可读性
*---------------------------------------------------------------------**/
int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, LctFs_t* result);
/**----------------------------------------------------------------------
* Function : GnssCalHeading
* Description : 获取GPS偏航角(0 - 360°)
* Date : 2022/09/16 logzhan
* 代码的可读性
*---------------------------------------------------------------------**/
double GnssCalHeading(GNSS_t* pgnss);
/**----------------------------------------------------------------------
* Function : calPdrHeadingOffset
* Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移
* Date : 2020/07/08 logzhan
*---------------------------------------------------------------------**/
void CalPdrHeadingOffset(Nmea_t* nmea_data, PDR_t* p_pdr);
/**---------------------------------------------------------------------
* Function : pdr_resetSysStatus
* Description : PDS 重设系统状态
* Date : 2020/2/1 logzhan
*---------------------------------------------------------------------**/
void ResetSystemStatus(EKFPara_t* kf);
/**----------------------------------------------------------------------
* Function : pdr_gnssInsLocation
* Description : PDR GPS融合INS惯性定位
* Date : 2021/01/29 logzhan
*---------------------------------------------------------------------**/
int GnssInsFusionLocation(Nmea_t* nmea_data, EKFPara_t* kf, LctFs_t* result);
/**---------------------------------------------------------------------
* Function : pdr_initGnssInfo
* Description : PDS GNSS定位信息初始化
* Date : 2020/2/1 logzhan
*---------------------------------------------------------------------**/
void InitGnssInfo(void);
/**----------------------------------------------------------------------
* 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
}
#endif
#endif

43
1.Software/PDR 1.06/include/LocationTool.h Normal file
View File

@ -0,0 +1,43 @@
#pragma once
#ifndef _LOCATION_TOOL_H_
#define _LOCATION_TOOL_H_
#include <stddef.h>
#ifndef PI
//#define PI 3.14159265358979323846
#endif // !PI
#ifndef EARTH_RADIUS
#define EARTH_RADIUS 6371008.8
#endif
typedef struct DoublePair {
double x;
double y;
} DoublePair;
typedef DoublePair PlaneCoordinate;
typedef struct LatLng{
double lat;
double lon;
} LatLng_t;
typedef struct DoublePairList{
double *x;
double *y;
size_t length;
} DoublePairList;
typedef struct LocationList{
double *lat;
double *lon;
size_t length;
} LocationList;
LatLng ProjPointOfLatLng(LatLng point, LatLng linePointA, LatLng linePointB);
DoublePair ProjPoint(DoublePair point, DoublePair linePointA, DoublePair linePointB);
//PlaneCoordinate WGS84_XYZ(const LatLng lla);
double CalculateDistance(LatLng pointA, LatLng pointB);
#endif // !_LOCATION_TOOL_H_

85
1.Software/PDR 1.06/include/Main.h Normal file
View File

@ -0,0 +1,85 @@
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : pdr_main.h
* Department : Sensor Algorithm Team
* Current Version : V2.0
* Author :
& yuanlin@vivo.cm
* Date of Issued : 2020.7.18
* Comments : PDR导航算法平台配置文件
********************************************************************************/
#ifdef __cplusplus
extern "C" {
#endif
#ifndef _PDR_MAIN_H_
#define _PDR_MAIN_H_
#include <string>
using namespace std;
#define PATH_MAX 256
#define TRACK_MAX 100000
typedef struct LatLngd {
double lat;
double lon;
double heading;
double hdop;
double accuracy;
double vel;
double time;
int motionType;
} LatLngd;
typedef struct ResultTracks {
LatLngd gpsTrack[TRACK_MAX];
LatLngd pdrTrack[TRACK_MAX];
int gpsLen;
int pdrLen;
}ResultTracks;
/**----------------------------------------------------------------------
* Function : pdr_writeKml
* Description : 将pdr算法输出的gps和pdr轨迹写为kml形式
* path : kml文件的输出文件路径
* name : kml文件主体名称
* postfix ：在主体名称后面添加的后缀，用于区分类型或者版本
* Date : 2020/11/1 logzhan
*---------------------------------------------------------------------**/
void KmlWrite(string path, string name, string postfix);
/**----------------------------------------------------------------------
* Function : getSimulateFileFp
* Description : 给定文件名，以及文件路径，获取仿真文件的文件指针
* Date : 2021/01/25 logzhan
*---------------------------------------------------------------------**/
FILE* getSimulateFile(FILE* catalogFp, string path_file, string& fileHead);
/**----------------------------------------------------------------------
* Function : gpsYaw2GoogleYaw
* Description : 为了让kml显示的角度方位正常，需要把0-360顺时针旋转的Yaw转换
* 为谷歌支持的Yaw规则
* Date : 2021/01/25 logzhan
*---------------------------------------------------------------------**/
double gpsYaw2GoogleYaw(double heading);
/**----------------------------------------------------------------------
* Function : Motion2TypeStr
* Description : 把用户运动类型转换为字符串输出
* Date : 2022/9/16 logzhan
*---------------------------------------------------------------------**/
const char* Motion2TypeStr(int type);
/**----------------------------------------------------------------------
* Function : updateResTrack
* Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹
* Date : 2021/01/25 logzhan
*---------------------------------------------------------------------**/
void UpdateResTrack(ResultTracks& resTrack, LctFs_t& lctfs);
#endif
#ifdef __cplusplus
}
#endif

70
1.Software/PDR 1.06/include/Matrix.h Normal file
View File

@ -0,0 +1,70 @@
#ifdef __cplusplus
extern "C" {
#endif
#ifndef _PDR_MATRIX_H_
#define _PDR_MATRIX_H_
#define N 4
/**---------------------------------------------------------------------
* Function : MatrixTrans
* Description : 矩阵转置
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void MatrixTrans(double a[N][N], double r[N][N]);
/**---------------------------------------------------------------------
* Function : VecMatMultiply
* Description : 向量和矩阵相乘 r = b * a
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void VecMatMultiply(double a[N], double b[N][N], double r[N]);
/**---------------------------------------------------------------------
* Function : MatrixMultiply
* Description : 矩阵和矩阵相乘 r = a * b
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void MatrixMultiply(double a[N][N], double b[N][N], double r[N][N]);
/**---------------------------------------------------------------------
* Function : MatrixAdd
* Description : 矩阵和矩阵相加 r = a + b, 注意这个函数支持a = a + b
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void MatrixAdd(double a[N][N], double b[N][N], double r[N][N]);
/**---------------------------------------------------------------------
* Function : VectorAdd
* Description : 向量和向量相加 r = a + b, 注意这个函数支持a = a + b
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void VectorAdd(double a[N], double b[N], double r[N]);
/**---------------------------------------------------------------------
* Function : MatrixSub
* Description : 矩阵和矩阵相减 r = a - b, 注意这个函数支持a = a - b
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void MatrixSub(double a[N][N], double b[N][N], double r[N][N]);
/**---------------------------------------------------------------------
* Function : VectorSub
* Description : 向量和向量相减 r = a - b, 注意这个函数支持a = a - b
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void VectorSub(double a[N], double b[N], double r[N]);
/**---------------------------------------------------------------------
* Function : MatrixInverse
* Description : 求矩阵的逆矩阵
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void MatrixInverse(double(*a)[N], double(*a_inv)[N]);
#endif
#ifdef __cplusplus
}
#endif

232
1.Software/PDR 1.06/include/PDRBase.h Normal file
View File

@ -0,0 +1,232 @@
/******************** (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

110
1.Software/PDR 1.06/include/ParseData.h Normal file
View File

@ -0,0 +1,110 @@
#ifdef __cplusplus
extern "C" {
#endif
#ifndef _PARSEDATA_H_
#define _PARSEDATA_H_
#include "pdr_sensor.h"
#define NAEA_LAST_TIME 500
#define IMU_LAST_COUNT 10
#define FLOAT_TO_INT 1000000
/**************************************************************************
* Description : NMEA数据和传感器数据解析
* : NMEA数据包括RMC,GGA,GSA,GSV,以及安卓定位信息语句
传感器数据包括加速度计，陀螺仪，磁力计
**************************************************************************/
char * strtok_ct(char * s, const char * delim);
/**************************************************************************
* Description : 解析惯性传感器数据
* Input : pt， 传感器数据字符串
ts， 手机开机时间戳，ms
sstp，安卓中传感器类型标识符
* Output : imu_p， 惯性传感器结构体
**************************************************************************/
void ParseIMU(char* pt, IMU_t *imu_p, double ts, int sstp);
/**************************************************************************
* Description : 解析NMEA数据
* Input : pt， NMEA数据字符串
ts， 手机开机时间戳，ms
* Output : ln， NMEA数据结构体
**************************************************************************/
void parseNMEA(char* pt, Nmea_t *ln, double ts);
/**************************************************************************
* Description : 解析GGA协议
* Input : pt， NMEA数据字符串
ts， 手机开机时间戳，ms
* Output : ln， NMEA数据结构体
**************************************************************************/
void ParseGGA(char* pt, Nmea_t *ln, double ts);
/**************************************************************************
* Description : 解析RMC协议
* Input : pt， NMEA数据字符串
ts， 手机开机时间戳，ms
* Output : ln， NMEA数据结构体
**************************************************************************/
void ParseRMC(char* pt, Nmea_t *ln, double ts);
/**************************************************************************
* Description : 解析GSV协议
* Input : pt， NMEA数据字符串
ts， 手机开机时间戳，ms
* Output : ln， NMEA数据结构体
**************************************************************************/
void parseGSV(char* pt, Nmea_t *ln, double ts);
/**************************************************************************
* Description : 解析GSA协议
* Input : pt， NMEA数据字符串
ts， 手机开机时间戳，ms
* Output : ln， NMEA数据结构体
**************************************************************************/
void ParseGSA(char* pt, Nmea_t *ln, double ts);
/**----------------------------------------------------------------------
* Function : parseLocAccuracy
* Description : 解析GPS的Accuracy精度参数
* Date : 2020/7/9 yuanlin@vivo.com &logzhan
*---------------------------------------------------------------------**/
void parseLocAccuracy(char* pt, Nmea_t *ln, double ts);
/**************************************************************************
* Description : 检测NMEA数据是否解析完整
* Input : ln， NMEA数据结构体
**************************************************************************/
void preprocessNMEA(Nmea_t *ln);
int ParseGnssInsData(char* line, IMU_t* imu_p, Nmea_t* ln);
/**----------------------------------------------------------------------
* Function : HexToDec
* Description : 十六进制数转换为十进制数
* Date : 2022-09-15 logzhan
*---------------------------------------------------------------------**/
long HexToDec(char *source);
/**----------------------------------------------------------------------
* Function : pdr_int2Hex
* Description : 十六进制数转换为十进制数
* Date : 2020/7/9 yuanlin@vivo.com & logzhan
*---------------------------------------------------------------------**/
char * Int2Hex(int a, char *buffer);
/**----------------------------------------------------------------------
* Function : pdr_getIndexOfSigns
* Description : 返回ch字符在sign数组中的序号
* Date : 2020/7/9 yuanlin@vivo.com &logzhan
*---------------------------------------------------------------------**/
int pdr_getIndexOfSigns(char ch);
#endif
#ifdef __cplusplus
}
#endif

140
1.Software/PDR 1.06/include/Pedometer.h Normal file
View File

@ -0,0 +1,140 @@
#pragma once
/******************** (C) COPYRIGHT 2023 Geek************************************
* File Name : Pedometer.h
* Department : Sensor Team
* Current Version : V1.0.0
* Author : logzhan
* Date of Issued : 2023.02.24
*******************************************************************************/
#ifdef __cplusplus
extern "C" {
#endif
#ifndef _STEPS_ALGORITHM_H_
#define _STEPS_ALGORITHM_H_
#include "pdr_sensor.h"
#define IMU_SAMPLE_FREQ 100 // IMU采样频率100Hz
#define MS2S 1000 // 秒转毫秒单位
#define DOWN_SAMPLE_NUM 4 // 降采样数量
#define ACC_SAMPLE_TIME MS2S / IMU_SAMPLE_FREQ // ACC采样间隔10ms
#define GYRO_SAMPLE_TIME MS2S / IMU_SAMPLE_FREQ // GYR采样间隔10ms
#define SAMPLE_TIME_MIN 9
#define PEDOMETER_FREQ IMU_SAMPLE_FREQ / DOWN_SAMPLE_NUM // 计步器的频率是
#define SampleFrequencyFour 100
#define SampleFrequencydouble 50
#define SampleFrequencyOPF (37.5l)
#define SampleFrequencySix 150
#define AVG_FILTER_BUFF_LEN 5 // 均值滤波器长度，默认为5
#define AXIS_NUM 4 // 轴的数量有4个: x、y、z、xyz
#define FILTER_NUM 4 // 滤波器数量4个
#define FILTER_AXIS_NUM AXIS_NUM * FILTER_NUM // 4*4=16, 轴数量4，滤波器数量4，总共16轴滤波器输出
#define AXIS_ALL 20 // AxisNumberFiltOut + FiltDifNumber
#define ACC_BUFF_NUM 39 // ACC缓存长度39
#define WIN_LENGTH_AVE_DEVI 50 //
#define PI2A (180/3.1415926)
#define STARTSTEPS 8
#define DET_VEHICLE_ACC_BUFF_LEN 200 // Origin acc sampleFrequency is 100 hz
#define WINDOW_LENGTH_MIN2MAX (DET_VEHICLE_ACC_BUFF_LEN/2 - 1) // 25hz , 1 second
#define PLATFORM_PC_WINDOWS
#define STEP_TRUE 1
#define STEP_FALSE 0
#define AXIS_XYZ 3
#if defined PLATFORM_ANDROID_QCOM_ADSP
#define STEPLIB_MEMSET SNS_OS_MEMSET
#if 1
#endif
#elif defined PLATFORM_ANDROID_QCOM_AP || defined PLATFORM_PC_WINDOWS || defined PLATFORM_PC_LINUX || defined PLATFORM_ANDROID_MTK || defined PLATFORM_ANDROID_QCOM_SLPI845
#define STEPLIB_MEMSET memset
#endif
#if defined PLATFORM_ANDROID_MTK
#define ABS_INT abs_value
#else
#define ABS_INT abs
#endif
typedef struct {
unsigned long walkSteps;
}stepsRe;
typedef struct {
double walk;
double walkRealTime;
double nD;
}StepsDefine_t;
typedef enum {
NonSteps = 0,
walk,
run,
}StepState_t;
typedef struct {
unsigned char sign;
StepState_t state;
unsigned long long stateStartTime;
unsigned long long stepsStartTime;
}stateNowDefine;
typedef struct {
StepState_t state;
unsigned long long startTimeStamp;
}StateDef_t;
typedef struct {
int flag;
double ax;
double ay;
double az;
}DownSample_t;
/**---------------------------------------------------------------------
* Function : Pedometer_GetVersion
* Description : 获取计步器版本
* Date : 2023/02/24
*---------------------------------------------------------------------**/
const char* Pedometer_GetVersion(void);
/**---------------------------------------------------------------------
* Function : initPedometer
* Description : PDR 初始化计步器
* Date : 2020/2/20 logzhan
*---------------------------------------------------------------------**/
void Pedometer_Init(void);
/**---------------------------------------------------------------------
* Function : Pedometer_ReInit
* Description : 重新初始化计步器
* Date : 2023/02/24 logzhan
*---------------------------------------------------------------------**/
void Pedometer_ReInit(void);
/**---------------------------------------------------------------------
* Function : PedometerUpdate
* Description : PDR 计步器的加速度输入, 并通过指针返回用户步数
* Date : 2023/02/25 logzhan 重构函数
*---------------------------------------------------------------------**/
void Pedometer_Update(IMU_t* imu, uint32_t* step);
void Pedometer_GetState(StepState_t* state);
/**---------------------------------------------------------------------
* Function : DetVehicleMode
* Description : PDR 检测汽车模式, 如果是检测到汽车模式，那么不计步数
* Date : 2023/02/24 logzhan
*---------------------------------------------------------------------**/
void DetVehicleMode(double ax, double ay, double az);
#endif
#ifdef __cplusplus
}
#endif

141
1.Software/PDR 1.06/include/Utils.h Normal file
View File

@ -0,0 +1,141 @@
#ifndef _PDR_UTIL_H_
#define _PDR_UTIL_H_
#ifdef __cplusplus
extern "C" {
#endif
#include <stdio.h>
#define TRAJ_POINT_COUNT 1000
#define MIN_POINT_COUNT 15
#define OPT_TRAJ_TIMEOUT 1000
#define FALIED_LINE 0
#define IN_LINE 1
#define CLOSE_START 2
#define CLOSE_END 3
typedef struct LatLng{
double lat;
double lon;
} LatLng_t;
typedef struct TrajSign
{
int count;
double lastTime;
double weight;
double yaw[TRAJ_POINT_COUNT];
double error[TRAJ_POINT_COUNT];
double points[TRAJ_POINT_COUNT][2];
}TrajSign;
/**地球相关参数*******************************************************************/
typedef struct
{
double rmh; /* 子午圈曲率半径 */
double rnh; /* 卯酉圈曲率半径 */
double grav; /* 当地重力加速度 */
double lat; /* 当地纬度（rad）*/
double wnie[3]; /* 地理系相对惯性系的角速度在导航系分量 */
} EarthData_t;
/**----------------------------------------------------------------------
* Function : mean
* Description : 对输入double数组求均值
* Review : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用
* 不同函数
* Date : 2020/7/4 logzhan
*---------------------------------------------------------------------**/
double mean(double *x, int n);
float fmean(float *data, int len);
float stdf(float* data, int len);
double meanAngle(double* angle, int len);
void modAngle(double * angle, double min, double max);
double pow_i(double num, long long n);
double pow_f(double num, double m);
double pow(double x, double y);
void centralization(double *x, double *x_out, int n);
void GetCovMatrix(double *x, double *y, double cov[2][2], int n);
int Jacobi(double a[][2], double p[][2], int n, double eps, int T);
LatLng_t ProjPointOfLatLng(LatLng_t point, LatLng_t linePointA, LatLng_t linePointB);
double CalDistance(LatLng_t pointA, LatLng_t pointB);
void ProjPointOfLatLng_cir(double point1[], double yaw, double point2[], double result[]);
/**----------------------------------------------------------------------
* Function : pdr_invSqrt
* Description : 计算1/sqrt(x)的快速的算法
* Date : 2020/6/30 logzhan
*---------------------------------------------------------------------**/
float InvSqrt(float x);
/**---------------------------------------------------------------------
* Function : pdr_v3Norm
* Description : 三维浮点数向量归一化
* Date : 2021/01/26 yuanlin@vivocom &&
*
*
*---------------------------------------------------------------------**/
void Vec3Norm(float* vx, float* vy, float* vz);
/**----------------------------------------------------------------------
* Function : pdr_writeCsvStr
* Description : 将字符串信息写入文本，主要是对fprintf封装一层
* Date : 2020/7/8 logzhan
*---------------------------------------------------------------------**/
void WriteCsvStr(FILE* fp_write, char* str);
/**----------------------------------------------------------------------
* Function : pdr_utc2hms
* Description : 将UTC时间转换为时分秒
* Date : 2020/7/8 logzhan
*---------------------------------------------------------------------**/
void pdr_utc2hms(double utc, double* h, double* m, double* s);
/**----------------------------------------------------------------------
* Function : qnb2att
* Description : 四元数转欧拉角
* Date : 2020/7/4 logzhan
*---------------------------------------------------------------------**/
void Qnb2Att(float* q, double* attitude);
double CalRadianDifferent(double s_dir, double d_dir);
/**----------------------------------------------------------------------
* Function : pdr_earthParameter
* Description : 根据输入纬度返回该纬度地球相关参数
* Date : 2020/7/8 logzhan
*---------------------------------------------------------------------**/
EarthData_t CalEarthParameter(double oriLat);
/**----------------------------------------------------------------------
* Function : Motion2TypeStr
* Description : 把用户运动类型转换为字符串输出
* Date : 2022/9/16 logzhan
*---------------------------------------------------------------------**/
const char* Motion2TypeStr(int type);
int pdr_min(int param_a, int param_b);
#ifdef __cplusplus
}
#endif
#endif

78
1.Software/PDR 1.06/include/buffer.h Normal file
View File

@ -0,0 +1,78 @@
#ifndef _PDR_BUFFER_H_
#define _PDR_BUFFER_H_
#ifdef __cplusplus
extern "C" {
#endif
/* Header File Including ------------------------------------------------------------------------ */
/* Macro Declaration ---------------------------------------------------------------------------- */
#define BUFFER_LONG_LENGTH 256
#define BUFFER_SHORT_LENGTH 10
#define BUFFER_TYPE_STACK 0
#define BUFFER_TYPE_QUEUE 1
#define BUFFER_NO_ERROR 0
#define BUFFER_WRONG_PARAMETER 1
#define BUFFER_OUT_OF_MEMORY 2
/* Structure Declaration ------------------------------------------------------------------------ */
#pragma pack (4)
typedef struct BUFFER {
char name[20];
int type;
int length;
int _bottom;
int _top;
int reserved;
double sum;
double mean;
float data[BUFFER_LONG_LENGTH + 1];
}Buffer_t;
typedef struct BUFFER_LONG {
char name[20];
int type;
int length;
int _bottom;
int _top;
int reserved;
double sum;
double mean;
float data[BUFFER_LONG_LENGTH + 1];
} BUFFER_LONG;
typedef struct BUFFER_SHORT {
char name[20];
int type;
int length;
int _bottom;
int _top;
int reserved;
double sum;
double mean;
float data[BUFFER_SHORT_LENGTH + 1];
} BUFFER_SHORT;
#pragma pack ()
/* Global Variable Declaration ------------------------------------------------------------------ */
/* Function Declaration ------------------------------------------------------------------------- */
int BufferInit(Buffer_t *buffer, const char *name, int type, int length);
int BufferClear(Buffer_t *buffer);
int BufferCount(Buffer_t *buffer, int *count);
int BufferGetTop(Buffer_t *buffer, float *value);
int BufferGetBottom(Buffer_t *buffer, float *value);
int BufferPop(Buffer_t *buffer, float *value);
int BufferPush(Buffer_t *buffer, float value);
int BufferGet(Buffer_t *buffer, float *value, int index);
int BufferMean(Buffer_t *buffer, float *mean);
int BufferVar(Buffer_t *buffer, float *var);
int BufferStd(Buffer_t *buffer, float *std);
#ifdef __cplusplus
}
#endif
#endif

15
1.Software/PDR 1.06/include/m_gps.h Normal file
View File

@ -0,0 +1,15 @@
#ifndef _PDR_GPS_H_
#define _PDR_GPS_H_
#ifdef __cplusplus
extern "C" {
#endif
#ifdef __cplusplus
}
#endif
#endif

19
1.Software/PDR 1.06/include/m_munkres.h Normal file
View File

@ -0,0 +1,19 @@
#ifndef __MUNKRES_H
#define __MUNKRES_H
#ifdef __cplusplus
extern "C" {
#endif
/* Macro Definition ----------------------------------------------------------------------------- */
#define MUNKRES_NO_ERROR 0
#define MUNKRES_EXCESSIVE_MATRIX 1
/* Function Declaration ------------------------------------------------------------------------- */
extern int MUNKRES_get_assignment(int *assignment, float* cost, int m, int n);
#ifdef __cplusplus
}
#endif
#endif // for __MUNKRES_H

151
1.Software/PDR 1.06/include/pdr_api.h Normal file
View File

@ -0,0 +1,151 @@
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : pdr_api.h
* Department : Sensor Algorithm Team
* Current Version : V2.0(compare QCOM SAP 5.0)
* Author : logzhan
* Date of Issued : 2021.01.26
* Comments : PDR 对外部接口
********************************************************************************/
#ifndef _PDR_API_H_
#define _PDR_API_H_
#ifdef __cplusplus
extern "C" {
#endif
/* Header File Including -----------------------------------------------------*/
#include <stdio.h>
#include <math.h>
#include <string.h>
#include "pdr_sensor.h"
#include "PDRBase.h"
#include "Utils.h"
/* Macro Definition ----------------------------------------------------------*/
#define IMU_LAST_COUNT 10
typedef struct _PosFusion{
double lat;
double lon;
double gpsLat;
double gpsLon;
double t;
int vaild;
}PosFusion;
/* Function Declaration ------------------------------------------------------*/
/**---------------------------------------------------------------------
* Function : Algorithm_Init
* Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨
* 迹平滑窗口的初始化
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void Algorithm_Init(void);
/**---------------------------------------------------------------------
* Function : PDRLocationMainLoop
* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置
* 迹平滑窗口的初始化
* Date : 2022/10/16 logzhan
*---------------------------------------------------------------------**/
int PDRLocationMainLoop(IMU_t* ImuData, Nmea_t* NmeaData, LctFs_t* LocFusion);
/**---------------------------------------------------------------------
* Function : pdr_locationMainLoopStr
* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置
* 迹平滑窗口的初始化
* Date : 2020/07/03 logzhan
* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑
*---------------------------------------------------------------------**/
int ParseDataAndUpdate(char* line, LctFs_t* result);
/**---------------------------------------------------------------------
* Function : pdr_initRmc
* Description : 初始化RMC结构体
* Input : rmc， RMC结构体
* Date : 2020/7/3 logzhan
*---------------------------------------------------------------------**/
void RMC_Init(NmeaRMC_t * rmc);
/**---------------------------------------------------------------------
* Function : pdr_initNmeaFlg
* Description : 初始化GPS的NMEA协议的相关标志位
* Input : ln， NMEA结构体
* Date : 2020/7/3 logzhan
*---------------------------------------------------------------------**/
void NmeaFlag_Init(Nmea_t * ln);
/**---------------------------------------------------------------------
* Function : clearNmeaFlg
* Description : 清空Nmea标志位,功能和init相同
* Date : 2020/7/4 logzhan
*---------------------------------------------------------------------**/
void ClearNmeaFlg(Nmea_t * ln);
/**---------------------------------------------------------------------
* Function : pdr_saveGnssInfo
* Description : 保存GPS相关信息
* Date : 2020/7/3 logzhan
*---------------------------------------------------------------------**/
void SaveGnssInfo(Nmea_t* nmea_data, LctFs_t* result, FILE* fp_gps);
/**----------------------------------------------------------------------
* Function : GetPDRVersion
* Description : 获取pdr版本号
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
const char* GetPDRVersion(void);
/**----------------------------------------------------------------------
* Function : pdr_closeAlgorithm
* Description : 关闭PDR算法,释放资源
* Date : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
void Algorithm_DeInit();
#ifdef _WIN32 // 主要用于支持win32调试使用，用于dll接口导出
/**----------------------------------------------------------------------
* Function : LibPDR_Init
* Description : 初始化PDR算法(DLL调用)
* Date : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
__declspec(dllexport) void LibPDR_Init();
/**----------------------------------------------------------------------
* Function : PDR_StepUpdateGPS
* Description : 仿真器按照GPS单步执行(DLL调用)
* Date : 2022/9/16 logzhan
*---------------------------------------------------------------------**/
__declspec(dllexport) PosFusion LibPDR_StepUpdateGPS(int useGpsFlg);
/**----------------------------------------------------------------------
* Function : setSimulatorFileCsvPath
* Description : 设置仿真文件的路径(DLL调用)
* Date : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
__declspec(dllexport) void setSimulatorFileCsvPath(char* path);
/**----------------------------------------------------------------------
* Function : LibPDR_DeInit
* Description : 关闭pdr算法，释放相关内存，用于dll调用(DLL调用)
* Date : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
__declspec(dllexport) void LibPDR_DeInit();
/**----------------------------------------------------------------------
* Function : setRefGpsYaw
* Description : 设置GPS参考角度
* Date : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
__declspec(dllexport) void setRefGpsYaw();
#endif
#ifdef __cplusplus
}
#endif
#endif

111
1.Software/PDR 1.06/include/pdr_detector.h Normal file
View File

@ -0,0 +1,111 @@
#ifndef __DETECTOR_H
#define __DETECTOR_H
#ifdef __cplusplus
extern "C" {
#endif
/* Header File Including ------------------------------------------------------------------------ */
#include <stdint.h>
#include <stdlib.h>
#include <time.h>
#include "PDRBase.h"
#include "buffer.h"
#include "AHRS.h"
/* Macro Declaration ---------------------------------------------------------------------------- */
#define DETECTOR_NO_ERROR 0
#define DETECTOR_OUT_OF_MEMORY 1
#define MAG_BUF_LEN 256
#define MAG_LEN 6
/* Struct Declaration --------------------------------------------------------------------------- */
typedef void (*DETECTOR_update_callback)(Detector_t *detector);
/* Global Variable Declaration ------------------------------------------------------------------ */
extern BUFFER_SHORT g_acc_frq_buf[3];
extern BUFFER_SHORT g_acc_amp_buf[3];
extern BUFFER_SHORT g_gyr_frq_buf[3];
extern BUFFER_SHORT g_gyr_amp_buf[3];
/* Function Declaration ------------------------------------------------------------------------- */
/**---------------------------------------------------------------------
* Function : GetDetectorObj
* Description : 获取PDR运动分类器对象
* Date : 2020/02/16 logzhan
*---------------------------------------------------------------------**/
Detector_t *GetDetectorObj(void);
/**---------------------------------------------------------------------
* Function : Detector_Init
* Description : 初始化运动分类器，识别用户处于哪一种运动模式
* Date : 2022/09/23 logzhan
*---------------------------------------------------------------------**/
Detector_t* Detector_Init(void);
/**---------------------------------------------------------------------
* Function : DetectorReset
* Description : 重置PDR运动分类器
* Date : 2022/09/23 logzhan
*---------------------------------------------------------------------**/
void DetectorReset(void);
/**---------------------------------------------------------------------
* Function : DetectMotionType
* Description : pdr运动类型检测
* Date : 2020/7/20
*---------------------------------------------------------------------**/
int DetectMotionType(void);
/**---------------------------------------------------------------------
* Function : predict
* Description : 判断手机携带方式（静止、手持、摆手等）
* Date : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
int pdr_detectorPredict(float* feature);
/**----------------------------------------------------------------------
* Function : DetUserStatic
* Description : 检测用户是否处于静止状态
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
int DetUserStatic(PDR_t* g_pdr, GNSS_t* pgnss, unsigned long delSteps);
/**----------------------------------------------------------------------
* Function : detIsCarMode
* Description : 识别是否是车载模式
* Input : pgnss，GPS数据结构体
g_pdr，PDR结构体
delSteps， 与上次相比步数的变化量
time，计数器
* Output : int，车载模式标志位
* Date : 2021/01/28 logzhan
*---------------------------------------------------------------------**/
int DetectCarMode(GNSS_t* pgnss, PDR_t* g_pdr, unsigned long delSteps, int* time);
/**----------------------------------------------------------------------
* Function : detPdrToReset
* Description : 检测PDR系统是否需要重置，考虑到后续PDR推算失误，或者其他情况
* 重置PDR到GPS的位置。
* Date : 2021/01/28 logzhan
*---------------------------------------------------------------------**/
int DetectPdrToReset(double pdr_angle, int* gpscnt, unsigned long deltsteps, PDR_t* g_pdr);
/**---------------------------------------------------------------------
* Function : DetectorUpdateIMU
* Description : 更新运动类型检测器的imu信息，如果到达一定的时间间隔，则会检测
* 用户的运动类型
* Date : 2022/09/23
*---------------------------------------------------------------------**/
void DetectorUpdateIMU(IMU_t* imu, PDR_t* pdr);
#ifdef __cplusplus
}
#endif
#endif

314
1.Software/PDR 1.06/include/pdr_sensor.h Normal file
View File

@ -0,0 +1,314 @@
/******************** (C) COPYRIGHT 2021 Geek************************************
* File Name : pdr_sensor.h
* Department : Sensor Algorithm Team
* Current Version : V2.0
* Author : logzhan
* Date of Issued : 2021.02.01
* Comments : PDR算法传感器基本定义
********************************************************************************/
#ifndef _SENSOR_H_
#define _SENSOR_H_
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
/* Macro Definition ----------------------------------------------------------*/
#define IMU_SENSOR_AXIS 3
#ifdef _WIN32
#define PDR_LOGD printf
#define PDR_LOGE printf
#else
#include <android/log.h>
#define LOG_TAG "Loc_PDR"
#define PDR_LOGD(...) __android_log_print(ANDROID_LOG_DEBUG, LOG_TAG, __VA_ARGS__)
#define PDR_LOGE(...) __android_log_print(ANDROID_LOG_ERROR, LOG_TAG, __VA_ARGS__)
#endif
#define ADD_POINT_NUM 10
#define GGA_STR "GGA"
#define RMC_STR "RMC"
#define GSA_STR "GSA"
#define GSV_STR "GSV"
#define GNGGA_STR "$GNGGA"
#define GPGGA_STR "$GPGGA"
#define GNRMC_STR "$GNRMC"
#define GPRMC_STR "$GPRMC"
#define GNGSA_STR "$GNGSA"
#define GPGSA_STR "$GPGSA"
#define GAGSA_STR "$GAGSA"
#define GLGSA_STR "$GLGSA"
#define GPGSV_STR "$GPGSV"
#define GAGSV_STR "$GAGSV"
#define GLGSV_STR "$GLGSV"
#define NORTH "N"
#define SOUTH "S"
#define EAST "E"
#define WEST "W"
#define ITEM_INVALID (-1)
#define INVAILD_GPS_YAW (-1) // 无效GPS航向角定义
/* Struct Definition ---------------------------------------------------------*/
typedef enum {
SENSOR_MIN,
SENSOR_ACC = 1,
SENSOR_MAG = 2,
SENSOR_GYRO = 4,
SENSOR_LIGHT = 5,
SENSOR_PROXIMITY = 8,
SENSOR_GAME_ROTATION_VECTOR = 15,
SENSOR_LOCATION_STATELLITE = 70,
SENSOR_LOCATION_NETWORK = 71,
SENSOR_LOCATION_FUSION = 72,
SENSOR_LOCATION_NMEA = 73,
SENSOR_LOCATION_NMEA_GNGGA = 74,
SENSOR_LOCATION_NMEA_GPGGA = 75,
SENSOR_LOCATION_NMEA_GNRMC = 76,
SENSOR_LOCATION_NMEA_GPRMC = 77,
SENSOR_LOCATION_NMEA_GNGSA = 78,
SENSOR_LOCATION_NMEA_GPGSA = 79,
SENSOR_LOCATION_NMEA_GPGSV = 80,
SENSOR_LOCATION_NMEA_GLGSV = 81,
SENSOR_LOCATION_NMEA_GAGSV = 82,
SENSOR_LOCATION_NMEA_GLGSA = 83,
SENSOR_LOCATION_NMEA_GAGSA = 84,
SENSOR_MAX,
} sensor;
typedef enum {
POSITIONING_N = 0,
POSITIONING_Y = 1,
}lct_status;
typedef enum {
POSITIONING_MODE_A = 0,
POSITIONING_MODE_M = 1,
}lct_mode;
typedef enum PDR_Mode {
RAW_GPS = 1,
STOP = 2,
WALK_PDR = 3,
WALK_GPS = 4,
CAR = 5
} PDR_Mode;
typedef enum DIR_Mode {
GPS_DATASHEET = 0,
IMU_PCA = 1,
DELTA_LOCATION = 2
} DIR_Mode;
/*for GSA*/
typedef enum {
PSTN_GSA_N = 1,
PSTN_GSA_Y_2D = 2,
PSTN_GSA_Y_3D = 3,
}lct_type;
typedef enum {
LATITUDE_N = 0,
LATITUDE_S = 1,
LONGITUDINAL_E = 2,
LONGITUDINAL_W = 3,
}LL_NSEW;
typedef enum {
MAG_E = 0,
MAG_W = 1,
}mag_heading;
typedef enum {
MODE_A = 0,
MODE_D = 1,
MODE_E = 2,
MODE_N = 3,
}gnrmc_mode;
typedef enum {
invalid = 0,
fix = 1,
dgps_fix = 2,
invalid_pps = 3,
rtk_fix = 4,
rtk_float = 5,
estimating = 6,
}gngga_quality;
typedef struct Sensor {
unsigned char update;
int type;
double time;
float s[IMU_SENSOR_AXIS];
}Sensor_t;
typedef struct imu {
Sensor_t acc;
Sensor_t gyr;
Sensor_t mag;
}IMU_t;
typedef struct NmeaRMC_t {
unsigned char update;
char rmc_check_sum[9];
sensor type;
lct_status status;
LL_NSEW rmc_latitude_ns;
LL_NSEW longitudinal_ew;
mag_heading mag_h;
gnrmc_mode mode;
float speed;
float cog;
float utc_ddmmyy;
float magnetic;
double rmc_utc;
double latitude;
double longitudinal;
double time; //ms
}NmeaRMC_t;
typedef struct NmeaGGA{
unsigned char update;
char gga_check_sum[9];
sensor type;
LL_NSEW gga_latitude_ns;
LL_NSEW longitudinal_ew;
gngga_quality status;
int satellites_nb;
int unit_a;
int unit_h;
int dgps;
float hdop;
float altitude;
float height;
double gga_utc;
double latitude;
double longitudinal;
double time; //ms
}NmeaGGA_t;
typedef struct NmeaGSA {
unsigned char update;
char check_sum[9];
sensor type;
lct_mode mode;
lct_type p_type;
int prnflg;
int prn[12];
float pdop;
float hdop;
float vdop;
double time; //ms
}NmeaGSA_t;
typedef struct satellite_status {
int prn;
int elevation; // elevation >10
int azimuth;
int snr; //snr >25 good
}satellite_status;
typedef struct NmeaGSV {
unsigned char update;
sensor type;
int sentences;
int sentence_number;
int satellites;
int satellite_number;
satellite_status satellites_data[20];
double time; //ms
}NmeaGSV_t;
typedef struct Accuracy{
unsigned char update;
int status; //-1,:Status Unknown; 0: Out of Service; 1: Temporarily Unavailable; 2: Available
float err; // m
double time;
}Accuracy_t;
typedef struct Nmea_t {
unsigned char update;
double minTime;
double maxTime;
NmeaGGA_t gga;
NmeaRMC_t rmc[2]; // 0 - GNRMC, 1 - GPRMC
NmeaGSA_t gsa[3]; // 0 - GPGSA, 1 - GLGSA, 2 - GAGSA
NmeaGSV_t gsv[3]; // 0 - GPGSV, 1 - GLGSV, 2 - GAGSV
Accuracy_t accuracy;
}Nmea_t;
typedef struct lct_fs {
LL_NSEW latitude_ns;
LL_NSEW longitudinal_ew;
double latitude;
double longitudinal;
double gpsLat;
double gpsLon;
double gpsHeading;
double pdrHeading;
double hdop;
double gpsSpeed;
double accuracy;
double time;
double yaw;
double lambda;
double last_lat;
double last_lon;
unsigned long step;
uint8_t motionType;
}LctFs_t;
typedef struct gnss {
LL_NSEW lat_ns;
LL_NSEW lon_ew;
int minElevation;
int satellites_num; // 当前卫星数量
int quality;
float yaw; // GNSS 航向角
float vel; // GNSS 速度
float HDOP; // GNSS 水平精度因子
float error; // 当前GPS的Accuracy
float lastError; // 上一次GPS的Accuracy
int overVelCount; // 速度过快数量统计, 记录速度过快的数量，用于判断手机是否处于车载模式
double timestamps;
double lat;
double lon;
double xNed;
double yNed;
}GNSS_t;
typedef struct IMU {
double time;
float gyr[3];
float acc[3];
float mag[3];
} IMU;
typedef struct AHRS {
uint8_t status;
uint8_t stable; // 当前AHRS算法收敛稳定性
float error;
float q[4]; //
float gravity[3];
float x_axis[3];
float y_axis[3];
float z_axis[3];
float Dt;
float Kp; // mahony kp比例调节参数
float Yaw;
float Pitch;
float Roll;
float insHeading;
}AHRS_t;
#ifdef __cplusplus
}
#endif
#endif

158
1.Software/PDR 1.06/include/scene_recognition.h Normal file
View File

@ -0,0 +1,158 @@
/*********************************************************************************/
/* FILE NAME : scene_recognition.h */
/* AUTHOR : Zhang Jingrui, ID: 11082826, Version: V0.1_Beta, Data:2019-03-28 */
/* DESCRIPTION: This header file conterns the prototype description of the external */
/* interface funtions of the scene recognition module and the definit- */
/* ion of related resources. */
/* HISTORY : (NONE) */
/********************************************************************************/
#ifndef _PDR_SCENE_RECOGNITION_H_
#define _PDR_SCENE_RECOGNITION_H_
/* Header Files Including -----------------------------------------*/
/*=================================================================*/
#include "pdr_sensor.h"
/* Macro Declaration ----------------------------------------------*/
/*=================================================================*/
#define _TIME_BUFFER_SIZE 10 /* Buffer size for the time calculation. */
#define _ACCURACY_BUFFER_SIZE 9 /* Accuracy time-based buffer size. */
#define _GNSS_INDEX_LEN 103 /* Structural array size in _GnssInfo. */
/* Extern Variable Declaration ------------------------------------*/
/*=================================================================*/
//extern int scene_recognition_debug_flag ;
//extern int scene_recognition_log_debug_flag ;
/* Status Declaration ---------------------------------------------*/
/*=================================================================*/
/* Usage: 场景识别模块初始化、重置操作结果状态标志位 */
typedef enum SCENE_INIT_STATE
{
// 场景识别模块初始化相关状态
INIT_SUCCESS = 0,
INIT_NOT_PERFORMED,
INIT_ERROR_MEMORY_OCCUPIED, // 模块资源未提前释放，需先释放资源
INIT_ERROR_MEMORY_INIT_FAILED, // 初始化内存申请失败
INIT_ERROR_REDUNDANCY, // 模块重复初始化，此时初始化无效。若需要重启模块，可以：
// 方法1：先关闭模块，关闭成功后，重新初始化模块。
// 方法2：重启模块，效果同方法1。
// 场景识别模块重置相关状态(for debug)
INIT_RESET_SUCCESS = 0,
INIT_RESET_FAILED
} SCENE_INIT_STATE;
/* Usage: 场景分类结果 */
typedef enum SCENE_RECOGNITION_RESULT
{
RECOG_UNKNOWN, // 未能确定场景的状态(默认状态)
RECOG_OPEN_AREA, // 场景open-area
RECOG_OTHERS, // 除了上述确定场景外的其他场景
RECOG_ERROR_PARAM_INCRECT, // 输入参数检查错误
RECOG_ERROR_NMEA_INFO_MISS, // 输入参数正确但是内容有误
/* 预研检测结果(不稳定) */
RECOG_MULTIPATH // 检测多径
} SCENE_RECOGNITION_RESULT;
/* Usage: 场景识别模块资源释放操作结果 */
typedef enum SCENE_DESTROY_STATE
{
DESTROY_SUCCESS = 0,
DESTROY_FIALED,
DESTROY_ERROR,
DESTROY_INVALID // 无效释放操作，表示没有对应的初始化或重置操作
} SCENE_DESTROY_STATE;
/* Status Declaration--------------------------------------------*/
/*===============================================================*/
// 描述当前场景识别模块的工作状态
typedef enum _SCENE_MODEL_WORK_STATE
{
MODEL_OFF,
MODEL_ON
} _SCENE_MODEL_WORK_STATE;
/* Usage: Identifier of the quality of the satellites' signal source. */
typedef enum _SIGNAL_QUALITY
{
SIG_UNKNOWN,
GOOD,
BAD
} SIGNAL_QUALITY;
/* Usage: extract and save information from Nmea_t for the module. */
/* This is the interface for the Nmea_t. */
typedef struct _GnssInfo
{
int update; // GNSS更新标志位 1：更新 0：不更新
double local_timestamp; // 当地时间
float accuracy; // GPS 输出的Accuracy参数，Accuracy > 0 当其为负数表示无效
int sat_visible_number; // 当前时间可见卫星数量
float snr_list[_GNSS_INDEX_LEN]; // 当前时间可见卫星的SNR列表
int sat_used_list[_GNSS_INDEX_LEN]; // 当前使用卫星列表
} GnssInfo;
/* Usage: 标记GNSS中的各个卫星系统 */
/* Struct Declaration -------------------------------------------*/
/*===============================================================*/
/* Function Declaration -------------------------------------------*/
/*=================================================================*/
#ifdef __cplusplus
extern "C" {
#endif
/* Function Name: sceneRecognitionInit() */
/* Usage: Initialization module for configuring related resources */
/* and must be executed once first beforing using the scene */
/* recognition module funcition. */
/* Param @ (NONE) */
/* Return @ init_res: Result state of the initialization process. */
SCENE_INIT_STATE SceneRecog_Init(void);
/* Function Name: scene_recognition_reset() */
/* Usage: 初始化模块，配置相关资源，在使用模块前必须首先被执行。 */
/* Param @ (NONE) */
/* Return @ init_res: 重置执行结果。 */
SCENE_INIT_STATE scene_recognition_reset(void);
/**----------------------------------------------------------------------
* Function : sceneRecognitionProc
* Description : GNSS场景识别处理流程：
* 1）将nmea转换为gnss
* 2) 通过accuracy、snr、卫星数量分析GNSS信号质量
* 3) 返回结果
* Input : PDR的nmea结构体
* Return : 场景识别结果
* Date : 2020/02/18
*---------------------------------------------------------------------**/
SCENE_RECOGNITION_RESULT sceneRecognitionProc(const Nmea_t* nmea);
/* Function Name: scene_recognition_destroy() */
/* Usage: 场景识别处理结束后的资源释放操作，资源释放后无法继续进行场 */
/* 景识别过程。 */
/* Param @ (NONE) */
/* Return @ destroy_state: 资源释放操作结果。 */
SCENE_DESTROY_STATE scene_recognition_destroy(void);
/**----------------------------------------------------------------------
* Function : isOpenArea
* Description : GNSS开阔地检测，通过输入GNSS信号，分析当前信号质量，从而确定
* 目标是否处于开阔场地。
* Return : 0 : 非开阔地 1：开阔地(信号质量较好)
* Author : Zhang Jingrui, Geek ID: 11082826
* Date : 2020/02/18 logzhan
*---------------------------------------------------------------------**/
int isOpenArea(const Nmea_t* nmea);
#ifdef __cplusplus
}
#endif
#endif //__SCENE_RECOGNITION_H__

207
1.Software/PDR 1.06/project/PDR.vcxproj Normal file
View File

@ -0,0 +1,207 @@
<?xml version="1.0" encoding="utf-8"?>
<Project DefaultTargets="Build" ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<ItemGroup Label="ProjectConfigurations">
<ProjectConfiguration Include="Debug|Win32">
<Configuration>Debug</Configuration>
<Platform>Win32</Platform>
</ProjectConfiguration>
<ProjectConfiguration Include="Debug|x64">
<Configuration>Debug</Configuration>
<Platform>x64</Platform>
</ProjectConfiguration>
<ProjectConfiguration Include="Release|Win32">
<Configuration>Release</Configuration>
<Platform>Win32</Platform>
</ProjectConfiguration>
<ProjectConfiguration Include="Release|x64">
<Configuration>Release</Configuration>
<Platform>x64</Platform>
</ProjectConfiguration>
</ItemGroup>
<ItemGroup>
<ClCompile Include="..\src\Buffer.c" />
<ClCompile Include="..\src\Complex.cpp" />
<ClCompile Include="..\src\CoordTrans.cpp" />
<ClCompile Include="..\src\Munkres.c" />
<ClCompile Include="..\src\AHRS.c" />
<ClCompile Include="..\src\Interface.c" />
<ClCompile Include="..\src\Detector.c" />
<ClCompile Include="..\src\Fft.c" />
<ClCompile Include="..\src\Filter.c" />
<ClCompile Include="..\src\Kalman.c" />
<ClCompile Include="..\src\LinearFit.c" />
<ClCompile Include="..\src\Location.c" />
<ClCompile Include="..\src\Main.cpp" />
<ClCompile Include="..\src\Matrix.c" />
<ClCompile Include="..\src\ParseData.c" />
<ClCompile Include="..\src\PDRBase.c" />
<ClCompile Include="..\src\Utils.c" />
<ClCompile Include="..\src\Quaternion.cpp" />
<ClCompile Include="..\src\SceneRecognition.c" />
<ClCompile Include="..\src\Pedometer.c" />
</ItemGroup>
<ItemGroup>
<ClInclude Include="..\include\buffer.h" />
<ClInclude Include="..\include\Filter.h" />
<ClInclude Include="..\include\LapProcess.h" />
<ClInclude Include="..\include\LocationTool.h" />
<ClInclude Include="..\include\m_munkres.h" />
<ClInclude Include="..\include\ParseData.h" />
<ClInclude Include="..\include\AHRS.h" />
<ClInclude Include="..\include\pdr_api.h" />
<ClInclude Include="..\include\PDRBase.h" />
<ClInclude Include="..\include\pdr_detector.h" />
<ClInclude Include="..\include\Fft.h" />
<ClInclude Include="..\include\Kalman.h" />
<ClInclude Include="..\include\LinearFit.h" />
<ClInclude Include="..\include\Location.h" />
<ClInclude Include="..\include\Main.h" />
<ClInclude Include="..\include\Matrix.h" />
<ClInclude Include="..\include\pdr_sensor.h" />
<ClInclude Include="..\include\pdr_trackSmooth.h" />
<ClInclude Include="..\include\Utils.h" />
<ClInclude Include="..\include\scene_recognition.h" />
<ClInclude Include="..\include\Pedometer.h" />
<ClInclude Include="..\src\Complex.h" />
<ClInclude Include="..\src\CoordTrans.h" />
<ClInclude Include="..\src\Quaternion.h" />
</ItemGroup>
<PropertyGroup Label="Globals">
<ProjectGuid>{28D32532-309F-40EA-9E4A-2D162CC434D2}</ProjectGuid>
<Keyword>Win32Proj</Keyword>
<RootNamespace>mp4spliter</RootNamespace>
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
<ProjectName>PDR</ProjectName>
</PropertyGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>true</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>true</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset>
<WholeProgramOptimization>true</WholeProgramOptimization>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
<ConfigurationType>Application</ConfigurationType>
<UseDebugLibraries>false</UseDebugLibraries>
<PlatformToolset>v142</PlatformToolset>
<WholeProgramOptimization>true</WholeProgramOptimization>
<CharacterSet>Unicode</CharacterSet>
</PropertyGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
<ImportGroup Label="ExtensionSettings">
</ImportGroup>
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<ImportGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="PropertySheets">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<ImportGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="PropertySheets">
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
</ImportGroup>
<PropertyGroup Label="UserMacros" />
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<LinkIncremental>true</LinkIncremental>
<IncludePath>..\include\;$(IncludePath)</IncludePath>
<ReferencePath>$(ReferencePath)</ReferencePath>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<LinkIncremental>true</LinkIncremental>
<IncludePath>..\3rdparty\Json\;..\3rdparty\onnx\;..\3rdparty\opencv\;..\include\;$(IncludePath)</IncludePath>
<ReferencePath>..\3rdparty\Json;$(ReferencePath)</ReferencePath>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
<LinkIncremental>false</LinkIncremental>
<IncludePath>..\include\;$(IncludePath)</IncludePath>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<LinkIncremental>false</LinkIncremental>
<IncludePath>..\include\;.\3rdparty\libjson\;..\3rdparty\libcurl\;..\3rdparty\opencv\;$(IncludePath)</IncludePath>
<LibraryPath>..\3rdparty\onnx\lib\;..\3rdparty\opencv\lib\;..\3rdparty\libcurl\bin\;..\3rdparty\libjson\bin\;$(LibraryPath)</LibraryPath>
<ReferencePath>D:\Program Files\OpenSSL-Win64\include;$(ReferencePath)</ReferencePath>
</PropertyGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<ClCompile>
<PrecompiledHeader>NotUsing</PrecompiledHeader>
<WarningLevel>Level3</WarningLevel>
<Optimization>Disabled</Optimization>
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;_LIB;_CRT_SECURE_NO_DEPRECATE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<SDLCheck>true</SDLCheck>
<BufferSecurityCheck>true</BufferSecurityCheck>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
<AdditionalDependencies>%(AdditionalDependencies)</AdditionalDependencies>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
<ClCompile>
<PrecompiledHeader>NotUsing</PrecompiledHeader>
<WarningLevel>Level3</WarningLevel>
<Optimization>Disabled</Optimization>
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;_LIB;_CRT_SECURE_NO_DEPRECATE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<SDLCheck>true</SDLCheck>
<BufferSecurityCheck>true</BufferSecurityCheck>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<PrecompiledHeader>NotUsing</PrecompiledHeader>
<Optimization>MaxSpeed</Optimization>
<FunctionLevelLinking>true</FunctionLevelLinking>
<IntrinsicFunctions>true</IntrinsicFunctions>
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;_LIB;_CRT_SECURE_NO_DEPRECATE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<SDLCheck>true</SDLCheck>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
<EnableCOMDATFolding>true</EnableCOMDATFolding>
<OptimizeReferences>true</OptimizeReferences>
</Link>
</ItemDefinitionGroup>
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
<ClCompile>
<WarningLevel>Level3</WarningLevel>
<PrecompiledHeader>NotUsing</PrecompiledHeader>
<Optimization>MaxSpeed</Optimization>
<FunctionLevelLinking>true</FunctionLevelLinking>
<IntrinsicFunctions>true</IntrinsicFunctions>
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;_LIB;_CRT_SECURE_NO_DEPRECATE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
<SDLCheck>true</SDLCheck>
</ClCompile>
<Link>
<SubSystem>Console</SubSystem>
<GenerateDebugInformation>true</GenerateDebugInformation>
<EnableCOMDATFolding>true</EnableCOMDATFolding>
<OptimizeReferences>true</OptimizeReferences>
<AdditionalDependencies>%(AdditionalDependencies)</AdditionalDependencies>
<LinkTimeCodeGeneration>UseLinkTimeCodeGeneration</LinkTimeCodeGeneration>
</Link>
</ItemDefinitionGroup>
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
<ImportGroup Label="ExtensionTargets">
</ImportGroup>
</Project>

159
1.Software/PDR 1.06/project/PDR.vcxproj.filters Normal file
View File

@ -0,0 +1,159 @@
<?xml version="1.0" encoding="utf-8"?>
<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<ItemGroup>
<Filter Include="src">
<UniqueIdentifier>{7c7c488d-9b32-4de4-9ec8-b4bd38c3e7b0}</UniqueIdentifier>
</Filter>
<Filter Include="inc">
<UniqueIdentifier>{a09dc878-ed5c-47e4-bdcc-c3e2c8f05882}</UniqueIdentifier>
</Filter>
<Filter Include="src\Utils">
<UniqueIdentifier>{1d99ec5e-847f-4166-8f79-ddb0473a04dd}</UniqueIdentifier>
</Filter>
<Filter Include="src\Math">
<UniqueIdentifier>{bd2be68d-d6d0-4b04-97d9-2ac99be07afe}</UniqueIdentifier>
</Filter>
<Filter Include="src\Location">
<UniqueIdentifier>{74391251-b6de-45ec-88b1-0ee0171e44da}</UniqueIdentifier>
</Filter>
<Filter Include="src\Ins">
<UniqueIdentifier>{7f1fc113-1a0a-488e-9f77-2b12c7280aed}</UniqueIdentifier>
</Filter>
</ItemGroup>
<ItemGroup>
<ClCompile Include="..\src\Buffer.c">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\Munkres.c">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\Interface.c">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\Detector.c">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\Fft.c">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\Filter.c">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\Main.cpp">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\SceneRecognition.c">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\Utils.c">
<Filter>src\Utils</Filter>
</ClCompile>
<ClCompile Include="..\src\Matrix.c">
<Filter>src\Math</Filter>
</ClCompile>
<ClCompile Include="..\src\Kalman.c">
<Filter>src\Location</Filter>
</ClCompile>
<ClCompile Include="..\src\LinearFit.c">
<Filter>src</Filter>
</ClCompile>
<ClCompile Include="..\src\Quaternion.cpp">
<Filter>src\Math</Filter>
</ClCompile>
<ClCompile Include="..\src\CoordTrans.cpp">
<Filter>src\Utils</Filter>
</ClCompile>
<ClCompile Include="..\src\Location.c">
<Filter>src\Location</Filter>
</ClCompile>
<ClCompile Include="..\src\ParseData.c">
<Filter>src\Utils</Filter>
</ClCompile>
<ClCompile Include="..\src\AHRS.c">
<Filter>src\Ins</Filter>
</ClCompile>
<ClCompile Include="..\src\Pedometer.c">
<Filter>src\Ins</Filter>
</ClCompile>
<ClCompile Include="..\src\Complex.cpp">
<Filter>src\Math</Filter>
</ClCompile>
<ClCompile Include="..\src\PDRBase.c">
<Filter>src</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<ClInclude Include="..\include\buffer.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\Filter.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\LapProcess.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\LocationTool.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\m_munkres.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\pdr_api.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\PDRBase.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\pdr_detector.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\Fft.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\LinearFit.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\Main.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\pdr_sensor.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\pdr_trackSmooth.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\scene_recognition.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\Utils.h">
<Filter>src\Utils</Filter>
</ClInclude>
<ClInclude Include="..\include\Kalman.h">
<Filter>src\Location</Filter>
</ClInclude>
<ClInclude Include="..\src\Quaternion.h">
<Filter>src\Math</Filter>
</ClInclude>
<ClInclude Include="..\src\CoordTrans.h">
<Filter>src\Utils</Filter>
</ClInclude>
<ClInclude Include="..\include\Matrix.h">
<Filter>src\Math</Filter>
</ClInclude>
<ClInclude Include="..\include\Location.h">
<Filter>src\Location</Filter>
</ClInclude>
<ClInclude Include="..\include\ParseData.h">
<Filter>src\Utils</Filter>
</ClInclude>
<ClInclude Include="..\include\AHRS.h">
<Filter>src\Ins</Filter>
</ClInclude>
<ClInclude Include="..\include\Pedometer.h">
<Filter>src\Ins</Filter>
</ClInclude>
<ClInclude Include="..\src\Complex.h">
<Filter>src\Math</Filter>
</ClInclude>
</ItemGroup>
</Project>

114
1.Software/PDR 1.06/src/AHRS.c Normal file
View File

@ -0,0 +1,114 @@
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : pdr_ahrs.c
* Department : Sensor Algorithm Team
* Current Version : V1.2
* Author : logzhan &
*
*
* Date of Issued : 2020.7.3
* Comments : PDR AHRS 姿态解算相关
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <math.h>
#include <string.h>
#include "PDRBase.h"
#include "AHRS.h"
#include "pdr_detector.h"
#include "Filter.h"
#include "buffer.h"
#include "Kalman.h"
#include "Utils.h"
#include "Quaternion.h"
/* Macro Definition ----------------------------------------------------------*/
#define FLT_THRES 0.000001f // 浮点数最小阈值
#define ENABLE_MAG 0
/* Global Variable Definition ------------------------------------------------*/
AHRS_t Ahrs;
/**----------------------------------------------------------------------
* Function : MahonyUpdateAHRS
* Description : Mahony姿态更新算法
* Date : 2020/02/18 logzhan :
* 增加了AHRS稳定性标志位，如果AHRS稳定后不再计算error, 降低运算
量。能提升PDR仿真10%的运行速度
*---------------------------------------------------------------------**/
static void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, float az,
float mx, float my, float mz) {
float* q = Ahrs.q;
// 归一化磁力计和加速度计
Vec3Norm(&ax, &ay, &az);
float wx = 0.0f, wy = 0.0f, wz = 0.0f;
#if ENABLE_MAG
if (!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
pdr_v3Norm(&mx, &my, &mz);
// 把磁场从载体系转换到地理坐标系 h = R^-1 * (mx,my,mz)
float hx = 2.0f * (mx * (0.5f - q2 * q2 - q[3] * q[3]) + my * (q[1] * q2 - q[0] * q[3]) + mz * (q[1] * q[3] + q[0] * q2));
float hy = 2.0f * (mx * (q[1] * q2 + q[0] * q[3]) + my * (0.5f - q[1] * q[1] - q[3] * q[3]) + mz * (q2 * q[3] - q[0] * q[1]));
float hz = 2.0f * (mx * (q[1] * q[3] - q[0] * q2) + my * (q2 * q[3] + q[0] * q[1]) + mz * (0.5f - q[1] * q[1] - q2 * q2));
// 理论上bx = 0, by 指向北向，因为手机IMU数据使用的是东北天(x,y,z)坐标系
float by = (float)sqrt(hx * hx + hy * hy);
float bz = hz;
wx = by * (q[0] * q[3] + q[1] * q2) + bz * (q[1] * q[3] - q[0] * q2);
wy = by * (0.5f - q[1] * q[1] - q[3] * q[3]) + bz * (q[0] * q[1] + q2 * q[3]);
wz = by * (q2 * q[3] - q[0] * q[1]) + bz * (0.5f - q[1] * q[1] - q2 * q2);
}
#endif
// 把重力转换到地理坐标系 v = R * (0,0,1)
float vx = q[1]*q[3] - q[0]*q[2];
float vy = q[0]*q[1] + q[2]*q[3];
float vz = q[0]*q[0] - 0.5f + q[3]*q[3];
// 计算矢量叉乘误差
float ex = ay*vz - az*vy + my*wz - mz*wy;
float ey = az*vx - ax*vz + mz*wx - mx*wz;
float ez = ax*vy - ay*vx + mx*wy - my*wx;
// Apply proportional feedback
gx += Ahrs.Kp * ex;
gy += Ahrs.Kp * ey;
gz += Ahrs.Kp * ez;
// 龙格库塔法更新四元数
float qa = q[0]; float qb = q[1]; float qc = q[2];
q[0] += (-qb * gx - qc * gy - q[3] * gz) * 0.5f * (Ahrs.Dt);
q[1] += ( qa * gx + qc * gz - q[3] * gy) * 0.5f * (Ahrs.Dt);
q[2] += ( qa * gy - qb * gz + q[3] * gx) * 0.5f * (Ahrs.Dt);
q[3] += ( qa * gz + qb * gy - qc * gx) * 0.5f * (Ahrs.Dt);
// 四元数归一化
QuaternionNorm(&q[0], &q[1], &q[2], &q[3]);
// 四元数转欧拉角
Ahrs.Pitch = asinf(-2.0f * (q[3]*q[1] - q[0]*q[2])) * (180.0f / 3.141592f);
Ahrs.Yaw = atan2f(q[2]*q[1] + q[0]*q[3], 0.5f - q[2]*q[2] - q[3]*q[3]) * (180.0f / 3.141592f);
Ahrs.Roll = atan2f(q[2]*q[3] + q[0]*q[1], 0.5f - q[2]*q[2] - q[1]*q[1]) * (180.0f / 3.141592f);
}
void AHRS_Init(void) {
memset(&Ahrs, 0, sizeof(Ahrs));
Ahrs.Kp = AHRS_KP;
Ahrs.Dt = 1.0f / AHRS_SAMPLE_FREQ;
Ahrs.q[0] = 1.0f;
}
/**----------------------------------------------------------------------
* Function : UpdateAHRS
* Description : AHRS融合解算
* Date : 2022/10/18 logzhan
*---------------------------------------------------------------------**/
int UpdateAHRS(IMU_t* imu) {
// logzhan 21/02/06 : 感觉是acc输入精度会高一些
MahonyUpdateAHRS(imu->gyr.s[0], imu->gyr.s[1], imu->gyr.s[2],
imu->acc.s[0], imu->acc.s[1], imu->acc.s[2],
imu->mag.s[0], imu->mag.s[1], imu->mag.s[2]);
return PDR_TRUE;
}

201
1.Software/PDR 1.06/src/Buffer.c Normal file
View File

@ -0,0 +1,201 @@
/******************** (C) COPYRIGHT 2022 Geek************************************
* File Name : Buffer.c
* Current Version : V1.2
* Author : logzhan
* Date of Issued : 2022.10.15
* Comments : PDR数据缓存、支持数据的均值、方差、标准差、顶部、底部数据的获取
* 和计算
********************************************************************************/
/* Header File Including ------------------------------------------------------*/
#include <string.h>
#include <math.h>
#include "buffer.h"
/* Macro Declaration ----------------------------------------------------------*/
#define ADD_PTR(ptr, ofs) ((ptr + ofs) % (buffer->length + 1))
#define INC_PTR(ptr) ((ptr + 1) % (buffer->length + 1))
#define DEC_PTR(ptr) ((ptr + buffer->length) % (buffer->length + 1))
/* Function Definition --------------------------------------------------------*/
int BufferInit(Buffer_t *buf, const char *name, int type, int len) {
int i;
if (NULL == buf) {
return BUFFER_WRONG_PARAMETER;
}
memset(buf->data, 0, buf->length * sizeof(*(buf->data)));
for (i = 0; i < 20 - 1 && '\0' != name[i]; ++i) {
buf->name[i] = name[i];
}
buf->name[i] = '\0';
buf->type = type;
buf->length = len;
buf->_bottom = 0;
buf->_top = 0;
buf->mean = 0.0;
buf->sum = 0.0;
return BUFFER_NO_ERROR;
}
int BufferClear(Buffer_t *buffer) {
if (NULL == buffer) {
return BUFFER_WRONG_PARAMETER;
}
memset(buffer->data, 0, buffer->length * sizeof(*(buffer->data)));
buffer->_bottom = 0;
buffer->_top = 0;
buffer->mean = 0.0;
buffer->sum = 0.0;
return BUFFER_NO_ERROR;
}
int BufferCount(Buffer_t *buffer, int *count) {
if (NULL == buffer) {
return BUFFER_WRONG_PARAMETER;
}
*count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1);
return BUFFER_NO_ERROR;
}
int BufferGetTop(Buffer_t *buffer, float *value) {
*value = 0;
if (NULL == buffer) {
return BUFFER_WRONG_PARAMETER;
}
if (buffer->_top == buffer->_bottom) {
return BUFFER_OUT_OF_MEMORY;
}
*value = buffer->data[DEC_PTR(buffer->_top)];
return BUFFER_NO_ERROR;
}
int BufferGetBottom(Buffer_t *buffer, float *value) {
if (NULL == buffer) {
return BUFFER_WRONG_PARAMETER;
}
if (buffer->_top == buffer->_bottom) {
return BUFFER_OUT_OF_MEMORY;
}
*value = buffer->data[buffer->_bottom];
return BUFFER_NO_ERROR;
}
int BufferPop(Buffer_t *buffer, float *value) {
if (NULL == buffer) {
return BUFFER_WRONG_PARAMETER;
}
if (buffer->_top == buffer->_bottom) {
return BUFFER_OUT_OF_MEMORY;
}
switch (buffer->type) {
case BUFFER_TYPE_STACK:
buffer->_top = DEC_PTR(buffer->_top);
*value = buffer->data[buffer->_top];
break;
case BUFFER_TYPE_QUEUE:
default:
*value = buffer->data[buffer->_bottom];
buffer->_bottom = INC_PTR(buffer->_bottom);
break;
}
return BUFFER_NO_ERROR;
}
int BufferPush(Buffer_t *buffer, float value) {
if (NULL == buffer) {
return BUFFER_WRONG_PARAMETER; /* BUFFER_WRONG_PARAMETER值为1 */
}
buffer->data[buffer->_top] = value;
buffer->_top = INC_PTR(buffer->_top); /* INC_PTR为加1（超过buffer长度就循环加1）*/
if (buffer->_top == buffer->_bottom) {
buffer->_bottom = INC_PTR(buffer->_bottom);
return BUFFER_OUT_OF_MEMORY; /* BUFFER_OUT_OF_MEMORY值为2 */
}
else {
return BUFFER_NO_ERROR; /* BUFFER_NO_ERROR值为0 */
}
}
int BufferGet(Buffer_t *buffer, float *value, int index) {
if (NULL == buffer) {
return BUFFER_WRONG_PARAMETER;
}
if (index >= buffer->length) {
return BUFFER_OUT_OF_MEMORY;
}
*value = buffer->data[ADD_PTR(buffer->_bottom, index)];
return BUFFER_NO_ERROR;
}
int BufferMean(Buffer_t *buffer, float *mean) {
int index;
int count;
if (NULL == buffer) {
return BUFFER_WRONG_PARAMETER;
}
*mean = 0;
count = 0;
for (index = buffer->_bottom; index != buffer->_top; index = INC_PTR(index)) {
*mean += buffer->data[index];
++count;
}
if (0 != count) {
*mean /= count;
}
return BUFFER_NO_ERROR;
}
int BufferVar(Buffer_t *buffer, float *var) {
int index;
int count;
float mean;
if (NULL == buffer) {
return BUFFER_WRONG_PARAMETER;
}
mean = 0;
count = 0;
for (index = buffer->_bottom; index != buffer->_top; index = INC_PTR(index)) {
mean += buffer->data[index];
++count;
}
if (0 != count) {
mean /= count;
}
*var = 0;
for (index = buffer->_bottom; index != buffer->_top; index = INC_PTR(index)) {
*var += (buffer->data[index] - mean) * (buffer->data[index] - mean);
}
if (0 != count) {
*var /= count;
}
return BUFFER_NO_ERROR;
}
int BufferStd(Buffer_t *buffer, float *std) {
int ret = BufferVar(buffer, std);
*std = sqrtf(*std);
return ret;
}

74
1.Software/PDR 1.06/src/Complex.cpp Normal file
View File

@ -0,0 +1,74 @@
/******************** (C) COPYRIGHT 2022 Geek************************************
* File Name : Complex.c
* Current Version : V2.0
* Author : logzhan
* Date of Issued : 2022.10.15
* Comments : PDR复数运算库支持
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include "math.h"
#include "Complex.h"
/**---------------------------------------------------------------------
* Function : ComplexAbs
* Description : 复数球绝对值运算
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
float ComplexAbs(Complex_t c) {
return sqrtf(c.real * c.real + c.image * c.image);
}
/**---------------------------------------------------------------------
* Function : ComplexAdd
* Description : 复数加法运算
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
Complex_t ComplexAdd(Complex_t a, Complex_t b) {
Complex_t c = { 0 };
c.real = a.real + b.real;
c.image = a.image + b.image;
return c;
}
/**---------------------------------------------------------------------
* Function : ComplexSub
* Description : 复数减法运算
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
Complex_t ComplexSub(Complex_t a, Complex_t b) {
Complex_t c = { 0 };
c.real = a.real - b.real;
c.image = a.image - b.image;
return c;
}
/**---------------------------------------------------------------------
* Function : ComplexMul
* Description : 复数乘法运算
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
Complex_t ComplexMul(Complex_t a, Complex_t b) {
Complex_t c;
c.real = a.real * b.real - a.image * b.image;
c.image = a.real * b.image + a.image * b.real;
return c;
}
/**---------------------------------------------------------------------
* Function : ComplexPow
* Description : 复数指数运算
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
Complex_t ComplexPow(Complex_t c, int n) {
Complex_t p = { 1, 0 };
int i;
float real;
for (i = 0; i < n; ++i) {
real = p.real * c.real - p.image * c.image;
p.image = p.real * c.image + p.image * c.real;
p.real = real;
}
return p;
}

24
1.Software/PDR 1.06/src/Complex.h Normal file
View File

@ -0,0 +1,24 @@
#ifndef _PDR_COMPLEX_H_
#define _PDR_COMPLEX_H_
#ifdef __cplusplus
extern "C" {
#endif
typedef struct Complex {
float real;
float image;
} Complex_t;
float ComplexAbs(Complex_t c);
Complex_t ComplexAdd(Complex_t a, Complex_t b);
Complex_t ComplexSub(Complex_t a, Complex_t b);
Complex_t ComplexMul(Complex_t a, Complex_t b);
Complex_t ComplexPow(Complex_t c, int n);
#ifdef __cplusplus
}
#endif
#endif

74
1.Software/PDR 1.06/src/CoordTrans.cpp Normal file
View File

@ -0,0 +1,74 @@
/******************** (C) COPYRIGHT 2022 Geek************************************
* File Name : CoordTrans.cpp
* Current Version : V1.0
* Author : logzhan
* Date of Issued : 2022.09.20
* Comments : PDR 坐标系转换相关函数支持
********************************************************************************/
#include <math.h>
#include "CoordTrans.h"
void WGS842ECEF(double* plla, double* ecef) {
double f = 1 / 298.257223563;
double a = 6378136.49;
double b, e, roc;
b = a * (1 - f);
e = sqrt(pow(a, 2) - pow(b, 2)) / a;
roc = a / sqrt(1 - pow(e * sin(plla[0]), 2));
ecef[0] = (roc + plla[2]) * cos(plla[0]) * cos(plla[1]);
ecef[1] = (roc + plla[2]) * cos(plla[0]) * sin(plla[1]);
ecef[2] = (roc * (1 - e * e) + plla[2]) * sin(plla[0]);
}
void ECEF2WGS84(double* ecef, double* plla)
{
double a = 6378137;
double b = 6356752.314245179;
double e, ep, p, th, roc;
e = sqrt(pow(a, 2) - pow(b, 2)) / a;
b = sqrt(pow(a, 2) * (1 - pow(e, 2)));
ep = sqrt((pow(a, 2) - pow(b, 2)) / pow(b, 2));
p = sqrt(ecef[0] * ecef[0] + ecef[1] * ecef[1]);
th = atan2(a * ecef[2], b * p);
plla[1] = atan2(ecef[1], ecef[0]);
plla[0] = atan2((ecef[2] + pow(ep, 2) * b * pow(sin(th), 3)), (p - pow(e, 2) * a * pow(cos(th), 3)));
roc = a / sqrt(1 - pow(e * sin(plla[0]), 2));
plla[2] = p / cos(plla[0]) - roc;
}
void ECEF2NED(double* ecef, double* plla, double* ned)
{
ned[0] = -sin(plla[0]) * cos(plla[1]) * ecef[0] - sin(plla[0]) * sin(plla[1]) * ecef[1] + cos(plla[0]) * ecef[2];
ned[1] = -sin(plla[1]) * ecef[0] + cos(plla[1]) * ecef[1];
ned[2] = -cos(plla[0]) * cos(plla[1]) * ecef[0] - cos(plla[0]) * sin(plla[1]) * ecef[1] - sin(plla[0]) * ecef[2];
}
void NED2ECEF(double* plla, double* ned, double* ecef0, double* ecef)
{
ecef[0] = -sin(plla[0]) * cos(plla[1]) * ned[0] - sin(plla[1]) * ned[1] - cos(plla[0]) * cos(plla[1]) * ned[2] + ecef0[0];
ecef[1] = -sin(plla[0]) * sin(plla[1]) * ned[0] + cos(plla[1]) * ned[1] - cos(plla[0]) * sin(plla[1]) * ned[2] + ecef0[1];
ecef[2] = cos(plla[0]) * ned[0] - sin(plla[0]) * ned[2] + ecef0[2];;
}
void WGS842NED(double* plla, double* ref_lla, double* ned)
{
double ecef[3] = { 0 };
WGS842ECEF(plla, ecef);
ECEF2NED(ecef, ref_lla, ned);
}
void NED2WGS84(double* ref_plla, double* ned, double* plla)
{
double ecef[3] = { 0 };
double ref_ecef[3] = { 0 };
WGS842ECEF(ref_plla, ref_ecef);
NED2ECEF(ref_plla, ned, ref_ecef, ecef);
ECEF2WGS84(ecef, plla);
}

24
1.Software/PDR 1.06/src/CoordTrans.h Normal file
View File

@ -0,0 +1,24 @@
#ifndef _PDR_COORD_SUPPORT_H
#define _PDR_COORD_SUPPORT_H
#ifdef __cplusplus
extern "C" {
#endif
void WGS842ECEF(double* plla, double* ecef);
void ECEF2WGS84(double* ecef, double* plla);
void ECEF2NED(double* ecef, double* plla, double* ned);
void NED2ECEF(double* plla, double* ned, double* ecef0, double* ecef);
void WGS842NED(double* plla, double* ref_lla, double* ned);
void NED2WGS84(double* ref_plla, double* ned, double* plla);
#ifdef __cplusplus
}
#endif
#endif // _PDR_KML_SUPPORT_H

726
1.Software/PDR 1.06/src/Detector.c Normal file
View File

@ -0,0 +1,726 @@
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : pdr_detector.c
* Department : Sensor Algorithm Team
* Current Version : V1.2
* Author : ZhangJingrui@vivo.com
* Date of Issued : 2020.7.4
* Comments : PDR检测器，主要用于区分用户的运动类型，用于PDR算法进行下一步的
* 决策。
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <string.h>
#include <math.h>
#include <stdio.h>
#include "PDRBase.h"
#include "pdr_detector.h"
#include "Filter.h"
#include "buffer.h"
#include "Fft.h"
#include "m_munkres.h"
/* Macro Definition ----------------------------------------------------------*/
#define FLT_BUF_LEN 256
#define FRQ_BUF_LEN 10
#define TMP_BUF_LEN 5
#define FRQ_RAT 0.3f
#define ACCE 0
#define GYRO 1
#define X_AXIS 0
#define Y_AXIS 1
#define Z_AXIS 2
#define ARR_LEN(arr) (sizeof(arr) / sizeof(*arr))
#define GET_MOL(x) (sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]))
#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]
/* Global Variable Definition ------------------------------------------------*/
static Detector_t pDetector;
static BUFFER_LONG g_mol_buf[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,
0.000496135120696701, 4.96135120696701e-05 };
const double g_mol_flt_a[] = { 1, -3.98765436730188, 8.09440659266207, -10.4762753571126, 9.42333716215302, -6.08421408355183,
2.83526165426630, -0.936403462626094, 0.208912324670401, -0.0283358586690797, 0.00176963186900233 };
const double g_mol_flt_h0 = 1;
BUFFER_SHORT g_acc_frq_buf[3];
BUFFER_SHORT g_acc_amp_buf[3];
static BUFFER_SHORT g_acc_tmp_buf[3];
BUFFER_SHORT g_gyr_frq_buf[3];
BUFFER_SHORT g_gyr_amp_buf[3];
static BUFFER_SHORT g_gyr_tmp_buf[3];
static int g_label[5] = { 0 };
/* Function Definition -------------------------------------------------------------------------- */
/**---------------------------------------------------------------------
* Function : GetDetectorObj
* Description : 获取PDR运动分类器对象
* Date : 2020/02/16 logzhan
*---------------------------------------------------------------------**/
Detector_t* GetDetectorObj(void) {
return &pDetector;
}
/**---------------------------------------------------------------------
* Function : Detector_Init
* Description : 初始化运动分类器，识别用户处于哪一种运动模式
* Date : 2020/02/16 logzhan & logzhan
*---------------------------------------------------------------------**/
Detector_t* Detector_Init(void) {
DetectorReset();
return &pDetector;
}
/**---------------------------------------------------------------------
* Function : DetectorReset
* Description : 重置PDR运动分类器
* Date : 2022/09/23 logzhan
*---------------------------------------------------------------------**/
void DetectorReset(void) {
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((void *)g_label, 0, sizeof(g_label));
}
/**---------------------------------------------------------------------
* Function : DetectorUpdateIMU
* Description : 更新运动类型检测器的imu信息，如果到达一定的时间间隔，则会检测
* 用户的运动类型
* Date : 2022/09/23
*---------------------------------------------------------------------**/
void DetectorUpdateIMU(IMU_t* imu, PDR_t* pdr) {
BufferPush((Buffer_t *)&g_mol_buf[ACCE], (float)FilterFilter(&g_mol_flt[ACCE], GET_MOL(imu->acc.s)));
BufferPush((Buffer_t *)&g_mol_buf[GYRO], (float)FilterFilter(&g_mol_flt[GYRO], GET_MOL(imu->gyr.s)));
// g_tick_d 实际上是1.28s检测一次，不然检测的频率过高
if ((pDetector.tick % (int)(1.28 * IMU_SAMPLING_FREQUENCY)) == 0) {
// 检测用户的运动类型
int detectResult = DetectMotionType();
if (detectResult == DETECTOR_NO_ERROR && pDetector.lastType != pDetector.type) {
pdr->MotionType = pDetector.type;
pDetector.lastType = pDetector.type;
}
}
++pDetector.tick;
}
/**---------------------------------------------------------------------
* Function : fft_buffer
* Description : 对acc数据做傅里叶变换，频率信息保存在fft中
* Date : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
static int fft_buffer(Complex_t *fft, Buffer_t *buffer) {
static float s_signal[1024] = { 0 };
float mean;
int index;
int i;
BufferMean(buffer, &mean);
for (i = 0, index = buffer->_bottom; index != buffer->_top; ++i, index = INC_PTR(buffer, index)) {
s_signal[i] = buffer->data[index] - mean; /*时序上255比0新*/
}
//序号i之后的长度里元素全部置为0
memset(s_signal + i, 0, (ARR_LEN(s_signal) - i) * sizeof(*s_signal));
if (0 == FFT_Fft(fft, s_signal, ARR_LEN(s_signal))) {
return DETECTOR_OUT_OF_MEMORY;
}
return DETECTOR_NO_ERROR;
}
/**----------------------------------------------------------------------
* Function : find_peaks
* Description : 获取前number个较大频点的频率frequency和幅度amplitude
* Date : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
static int find_peaks(float *frequency, float *amplitude, int number, Complex_t *fft,
int length, float cut_off_frequency, float min_threshold) {
float amp_2[2];
int min_ind;
int dir;
int n;
int i;
int j;
min_ind = 0;
memset(frequency, 0, number * sizeof(*frequency));
memset(amplitude, 0, number * sizeof(*amplitude));
dir = 1;
n = (int)(cut_off_frequency * length / IMU_SAMPLING_FREQUENCY);
amp_2[1] = fft[0].real * fft[0].real + fft[0].image * fft[0].image;
for (i = 1; i < n; ++i) {
amp_2[0] = amp_2[1];
amp_2[1] = fft[i].real * fft[i].real + fft[i].image * fft[i].image;
if (1 == dir) {
if (amp_2[1] < amp_2[0]) {
if (amp_2[0] > amplitude[min_ind]) {
amplitude[min_ind] = amp_2[0];
frequency[min_ind] = (float)i;
min_ind = 0;
for (j = 1; j < number; ++j) {
if (amplitude[min_ind] > amplitude[j]) {
min_ind = j;
}
}
}
dir = -1;
}
}
else {
if (amp_2[1] > amp_2[0]) {
dir = 1;
}
}
}
for (i = 0; i < number; ++i) {
amplitude[i] = (float)sqrt(amplitude[i]);
if (amplitude[i] < min_threshold) {
frequency[i] = 0;
amplitude[i] = 0;
}
else {
frequency[i] *= (float)IMU_SAMPLING_FREQUENCY / length;
}
}
return DETECTOR_NO_ERROR;
}
static void get_nonzero_weighted_mean(float *mean, BUFFER_SHORT *buffer, int length) {
int weight_sum;
int weight;
int index;
if (buffer->_bottom == buffer->_top) {
*mean = 0;
return;
}
*mean = 0;
weight_sum = 0;
index = buffer->_top;
for (weight = length; weight > 0 && index != buffer->_bottom; --weight) {
index = DEC_PTR(buffer, index);
if (0 != buffer->data[index]) {
*mean += weight * buffer->data[index];
weight_sum += weight;
}
}
if (0 != weight_sum) {
*mean /= weight_sum;
}
}
static void count_zeros(int *count, BUFFER_SHORT *buffer, int length) {
int index;
*count = 0;
for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) {
if (0 == buffer->data[index]) {
++*count;
}
}
}
static void copy_buffer(BUFFER_SHORT *dst, BUFFER_SHORT *src, int length) {
int dst_cnt;
int src_cnt;
int dst_idx;
int src_idx;
BufferCount((Buffer_t *)dst, &dst_cnt);
BufferCount((Buffer_t *)src, &src_cnt);
length = length < dst_cnt ? length : dst_cnt;
length = length < src_cnt ? length : src_cnt;
dst_idx = (dst->_top + dst->length + 1 - length) % (dst->length + 1);
src_idx = (src->_top + src->length + 1 - length) % (src->length + 1);
while (dst_idx != dst->_top) {
dst->data[dst_idx] = src->data[src_idx];
dst_idx = INC_PTR(dst, dst_idx);
src_idx = INC_PTR(src, src_idx);
}
}
static void clear_buffer(BUFFER_SHORT *buffer) {
int index;
index = buffer->_bottom;
while (index != buffer->_top) {
buffer->data[index] = 0;
index = INC_PTR(buffer, index);
}
}
static int track_frequency(BUFFER_SHORT *frequency_buffer, BUFFER_SHORT *amplitude_buffer, BUFFER_SHORT *temp_buffer,
float *frequency, float *amplitude, int number) {
static float mean[3];
static float cost[3 * 3];
static int assignment[3];
float distance;
float prev;
int i;
int j;
int transform_flag;
int count;
/* 0.detect argument */
if (number > ARR_LEN(mean)) {
return DETECTOR_OUT_OF_MEMORY;
}
/* 1.caculate main mean */
for (i = 0; i < number; ++i) {
get_nonzero_weighted_mean(&mean[i], &frequency_buffer[i], TMP_BUF_LEN);
}
/* 2.caculate main cost */
for (i = 0; i < number; ++i) {
for (j = 0; j < number; ++j) {
distance = (float)fabs(mean[i] - frequency[j]);
if (0 == mean[i] || 0 == frequency[j] || distance > FRQ_RAT) {
COST(i, j) = 1e3;
}
else {
COST(i, j) = distance;
}
}
}
/* 3.get main assignment */
MUNKRES_get_assignment(assignment, cost, number, number);
/* 4.push main frequency and main amplitude buffer */
for (i = 0; i < number; ++i) {
if (1e3 == COST(i, assignment[i])) {
BufferPush((Buffer_t *)&frequency_buffer[i], 0);
BufferPush((Buffer_t *)&amplitude_buffer[i], 0);
}
else {
BufferPush((Buffer_t *)&frequency_buffer[i], frequency[assignment[i]]);
BufferPush((Buffer_t *)&amplitude_buffer[i], amplitude[assignment[i]]);
frequency[assignment[i]] = 0;
amplitude[assignment[i]] = 0;
}
}
/* 5.caculate temp mean */
for (i = 0; i < number; ++i) {
get_nonzero_weighted_mean(&mean[i], &temp_buffer[i], TMP_BUF_LEN);
}
/* 6.caculate temp cost */
for (i = 0; i < number; ++i) {
for (j = 0; j < number; ++j) {
if (0 == frequency[j]) {
COST(i, j) = 1e3;
}
else if (0 == mean[i]) {
COST(i, j) = FRQ_RAT;
}
else {
COST(i, j) = (float)fabs(mean[i] - frequency[j]);
}
}
}
/* 7.get main assignment */
MUNKRES_get_assignment(assignment, cost, number, number);
/* 8.push temp buffer */
for (i = 0; i < number; ++i) {
BufferPush((Buffer_t *)&temp_buffer[i], frequency[assignment[i]]);
}
/* 9.caculate temp mean */
for (i = 0; i < number; ++i) {
get_nonzero_weighted_mean(&mean[i], &temp_buffer[i], TMP_BUF_LEN);
}
/* 10.check temp buffer */
transform_flag = 0;
for (i = 0; i < number; ++i) {
prev = temp_buffer[i].data[temp_buffer[i]._bottom];
for (j = temp_buffer[i]._bottom; j != temp_buffer[i]._top; j = INC_PTR(&temp_buffer[i], j)) {
if (0 == temp_buffer[i].data[j] || fabs(temp_buffer[i].data[j] - prev) > FRQ_RAT) {
break;
}
prev = temp_buffer[i].data[j];
}
if (j == temp_buffer[i]._top) {
frequency[i] = mean[i];
transform_flag = 1;
}
else {
frequency[i] = 0;
}
}
if (transform_flag) {
/* 11.caculate main mean */
for (i = 0; i < number; ++i) {
get_nonzero_weighted_mean(&mean[i], &frequency_buffer[i], TMP_BUF_LEN);
}
/* 12.caculate main cost */
for (i = 0; i < number; ++i) {
count_zeros(&count, &frequency_buffer[i], TMP_BUF_LEN);
for (j = 0; j < number; ++j) {
if (0 == frequency[j]) {
COST(i, j) = 1e3;
}
else if (0 == mean[i]) {
COST(i, j) = -5;
}
else if (count >= 3) {
// COST(i, j) = 3 - count;
COST(i, j) = (float)(-5 + fabs(mean[i] - frequency[j]));
}
else {
COST(i, j) = (float)fabs(mean[i] - frequency[j]);
}
}
}
/* 13.get main assignment */
MUNKRES_get_assignment(assignment, cost, number, number);
/* 14.push main frequency and main amplitude buffer */
for (i = 0; i < number; ++i) {
if (0 != frequency[assignment[i]]) {
// Buffer_push(&frequency_buffer[i], frequency[assignment[i]]);
// Buffer_push(&amplitude_buffer[i], 0);
copy_buffer(&frequency_buffer[i], &temp_buffer[assignment[i]], TMP_BUF_LEN);
clear_buffer(&temp_buffer[assignment[i]]);
}
}
}
return DETECTOR_NO_ERROR;
}
static void get_nonzero_mean(float *mean, BUFFER_SHORT *buffer, int length) {
int count;
int index;
count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1);
length = count < length ? count : length;
*mean = 0;
count = 0;
for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) {
if (0 != buffer->data[index]) {
*mean += buffer->data[index];
++count;
}
}
if (count >= 0.5 * length) {
*mean /= count;
}
}
static void get_nonzero_std(float *std, BUFFER_SHORT *buffer, int length) {
float mean;
float delta;
int count;
int index;
count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1);
length = count < length ? count : length;
mean = 0;
count = 0;
for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) {
if (0 != buffer->data[index]) {
mean += buffer->data[index];
++count;
}
}
if (0 != count) {
mean /= count;
}
*std = 0;
for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) {
if (0 != buffer->data[index]) {
delta = buffer->data[index] - mean;
*std += delta * delta;
}
}
if (count >= 0.5 * length) {
*std = (float)sqrt(*std / count);
}
else {
*std = -1; // default;
}
}
/**---------------------------------------------------------------------
* Function : get_feature
* Description : 获取用于判断手机携带方式（静止、手持、摆手等）的特征
* Date : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
static void get_feature(float *feature, int length) {
int i;
int j;
float temp;
BufferMean((Buffer_t *)&g_mol_buf[ACCE], &feature[0]);
BufferStd( (Buffer_t *)&g_mol_buf[ACCE], &feature[1]);
BufferStd( (Buffer_t *)&g_mol_buf[GYRO], &feature[2]);
get_nonzero_std( &feature[3] , &g_acc_frq_buf[0], length); if (feature[3] < 0) feature[3] = 10;
get_nonzero_std( &feature[4] , &g_acc_frq_buf[1], length); if (feature[4] < 0) feature[4] = 10;
get_nonzero_std( &feature[5] , &g_acc_frq_buf[2], length); if (feature[5] < 0) feature[5] = 10;
get_nonzero_mean(&feature[6] , &g_acc_amp_buf[0], length);
get_nonzero_mean(&feature[7] , &g_acc_amp_buf[1], length);
get_nonzero_mean(&feature[8] , &g_acc_amp_buf[2], length);
get_nonzero_std( &feature[9] , &g_gyr_frq_buf[0], length); if (feature[9] < 0) feature[9] = 10;
get_nonzero_std( &feature[10], &g_gyr_frq_buf[1], length); if (feature[10] < 0) feature[10] = 10;
get_nonzero_std( &feature[11], &g_gyr_frq_buf[2], length); if (feature[11] < 0) feature[11] = 10;
get_nonzero_mean(&feature[12], &g_gyr_amp_buf[0], length);
get_nonzero_mean(&feature[13], &g_gyr_amp_buf[1], length);
get_nonzero_mean(&feature[14], &g_gyr_amp_buf[2], length);
/* sort by std - bubble sort */
for (i = 0; i < 2; ++i) {
for (j = 0; j < 2 - i; ++j) {
if (feature[3 + j] > feature[3 + j + 1]) {
temp = feature[3 + j];
feature[3 + j] = feature[3 + j + 1];
feature[3 + j + 1] = temp;
temp = feature[6 + j];
feature[6 + j] = feature[6 + j + 1];
feature[6 + j + 1] = temp;
}
}
}
for (i = 0; i < 2; ++i) {
for (j = 0; j < 2 - i; ++j) {
if (feature[9 + j] > feature[9 + j + 1]) {
temp = feature[9 + j];
feature[9 + j] = feature[9 + j + 1];
feature[9 + j + 1] = temp;
temp = feature[12 + j];
feature[12 + j] = feature[12 + j + 1];
feature[12 + j + 1] = temp;
}
}
}
}
/**---------------------------------------------------------------------
* Function : predict
* Description : 判断手机携带方式（静止、手持、摆手等）
* Date : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
int pdr_detectorPredict(float *feature)
{
if (feature[1] < 0.932921) {
return DETECTOR_TYPE_STATIC;
}else if ((feature[3] < 0.10) + (feature[4] < 0.10) + (feature[5] < 0.10) > 0) {
if (feature[12] < 49.9795) {
if (feature[0] < 10.7220) {
return DETECTOR_TYPE_HANDHELD;
}
else {
return DETECTOR_TYPE_SWINGING;
}
}
else {
if (feature[2] < 0.312002) {
return DETECTOR_TYPE_HANDHELD;
}
else {
return DETECTOR_TYPE_SWINGING;
}
}
}
return DETECTOR_TYPE_IRREGULAR;
}
/**---------------------------------------------------------------------
* Function : debounce
* Description : 去抖动，判断之前length个label，根据较多的携带方式最终确定手
机携带方式
* Date : 2020/7/20 logzhan
*---------------------------------------------------------------------**/
static int debounce(int *label, int length) {
int cnt[4];
int def;
int i;
memset(cnt, 0, sizeof(cnt));
for (i = 0; i < length; ++i) {
++cnt[label[i]];
}
def = 0;
for (i = 0; i < ARR_LEN(cnt); ++i) {
if (cnt[i] > length * 3 / 4) {
return i;
}
if (cnt[i] > 0) {
def = i;
}
}
return def;
}
/**---------------------------------------------------------------------
* Function : DetectMotionType
* Description : pdr运动类型检测器
* Date : 2022/09/23 logzhan
*---------------------------------------------------------------------**/
int DetectMotionType() {
static Complex_t s_fft[1024] = { {0.0} };
static float s_feature[15] = { 0 };
float frq[3];
float amp[3];
int ret;
int i;
// 分析加速度计
if (DETECTOR_NO_ERROR != (ret = fft_buffer(s_fft, (Buffer_t *)&g_mol_buf[ACCE]))) {
return ret;
}
if (DETECTOR_NO_ERROR != (ret = find_peaks(frq, amp, 3, s_fft, 1024, 5, 0))) {
return ret;
}
if (DETECTOR_NO_ERROR != (ret = track_frequency(g_acc_frq_buf, g_acc_amp_buf, g_acc_tmp_buf, frq, amp, 3))) {
return ret;
}
// 分析陀螺仪
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 == 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))) {
return ret;
}
get_feature(s_feature, 5);
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] = pDetector.type;
pDetector.type = debounce(g_label, ARR_LEN(g_label));
return DETECTOR_NO_ERROR;
}
/**----------------------------------------------------------------------
* Function : DetUserStatic
* Description : 检测用户是否处于静止状态, 目前主要采用计步器判断和运动频率分析
* 单运动频率计算量太大，可以考虑用加速度判断用户状态。
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
int DetUserStatic(PDR_t* g_pdr, GNSS_t* pgnss, unsigned long delSteps) {
if (pgnss->vel != -1 && pgnss->vel < 1 && (delSteps == 0 &&
g_pdr->motionFreq == 0.0)) {
return PDR_TRUE;
}
return PDR_FALSE;
}
/**----------------------------------------------------------------------
* Function : DetectCarMode
* Description : 识别是否是车载模式
* Input : pgnss，GPS数据结构体
g_pdr，PDR结构体
delSteps， 与上次相比步数的变化量
time，计数器
* Output : int，车载模式标志位
* Date : 2021/01/28 logzhan
*---------------------------------------------------------------------**/
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 {
*time = 0;
}
if (*time > 5) {
return PDR_TRUE;
}
return PDR_FALSE;
}
/**----------------------------------------------------------------------
* Function : DetectPdrToReset
* Description : 检测PDR系统是否需要重置，考虑到后续PDR推算失误，或者其他情况
* 重置PDR到GPS的位置。
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
int DetectPdrToReset(double pdr_angle, int* gpsCnt, ulong deltsteps, PDR_t* pdr) {
if ((pdr_angle < 0 && deltsteps > 0) || (deltsteps > 0 && (pdr->motionFreq == 0))) {
(*gpsCnt)++;
}
else {
*gpsCnt = 0;
}
return (*gpsCnt > 6);
}

116
1.Software/PDR 1.06/src/Fft.c Normal file
View File

@ -0,0 +1,116 @@
/* Header File Including ------------------------------------------------------------------------ */
#include "Fft.h"
/* Macro Declaration ---------------------------------------------------------------------------- */
#define FFT_MCB_TYPE_OMEGA_AND_POSITION 0x01
#define SWAP(type, a, b) { type t = a; a = b; b = t; }
/* Structure Declaration ------------------------------------------------------------------------ */
typedef struct FFT_MCB {
uint8_t type;
uint32_t parameter[4];
struct FFT_MCB *next;
struct FFT_MCB *prev;
uint8_t buffer[10240];
} FFT_MCB;
/* Global Variable Definition ------------------------------------------------------------------- */
static FFT_MCB gMemery;
/* Function Definition -------------------------------------------------------------------------- */
/**----------------------------------------------------------------------
* Function : FftInit
* Description : 初始化傅里叶运算库
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
static int FftInit(int length) {
Complex_t *omega;
uint16_t *position;
int bitLength;
int i;
int shift;
int j;
if (length * (sizeof(Complex_t) + sizeof(uint16_t)) > sizeof(gMemery.buffer)) {
return 0;
}
if (FFT_MCB_TYPE_OMEGA_AND_POSITION == gMemery.type && length == (int)gMemery.parameter[0]) {
return length;
}
gMemery.type = FFT_MCB_TYPE_OMEGA_AND_POSITION;
gMemery.parameter[0] = length;
omega = (Complex_t *) gMemery.buffer;
for (i = 0; i < length; ++i) {
omega[i].real = (float)cos(-2 * M_PI * i / length);
omega[i].image = (float)sin(-2 * M_PI * i / length);
}
position = (uint16_t *)(omega + length);
for (bitLength = 0, i = length; (i >> bitLength); ++bitLength);
for (i = 0; i < length; ++i) {
j = 0;
for (shift = 0; shift < bitLength - 1; ++shift) {
if((i >> shift) & 1) {
j |= (1 << (bitLength - shift - 2));
}
}
position[i] = (uint16_t)j;
}
return length;
}
int FFT_Dft(Complex_t *dft, float *signal, int length) {
if (0 == FftInit(length)) return 0;
for (int k = 0; k < length; ++k) {
dft[k].real = 0;
dft[k].image = 0;
for(int i = 0; i < length; ++i) {
int ik = i * k % length;
dft[k].real += signal[i] * ((Complex_t *)gMemery.buffer)[ik].real;
dft[k].image += signal[i] * ((Complex_t *)gMemery.buffer)[ik].image;
}
}
return length;
}
int FFT_Fft(Complex_t *fft, float *signal, int length) {
int i,l,j,m;
uint16_t *position;
Complex_t *omega;
//正常情况下，length正常、没溢出就不会进入return0
if (length <= 0 || 0 != (length & (length - 1)) || 0 == FftInit(length)) {
return 0;
}
for (i = 0; i < length; ++i) {
fft[i].real = signal[i];
fft[i].image = 0;
}
position = (uint16_t *)(gMemery.buffer + length * sizeof(Complex_t));
for (i = 0; i < length; ++i) {
if (i < position[i]) {
SWAP(Complex_t, fft[i], fft[position[i]]);
}
}
omega = (Complex_t *)gMemery.buffer;
for (l = 2, m = 1; l <= length; l <<= 1, m <<= 1) {
for (i = 0; i < length; i += l) {
for (j = 0; j < m; ++j) {
Complex_t delta = ComplexMul(omega[length / l * j], fft[i + m + j]);
fft[i + m + j] = ComplexSub(fft[i + j], delta);
fft[i + j] = ComplexAdd(fft[i + j], delta);
}
}
}
return length;
}

77
1.Software/PDR 1.06/src/Filter.c Normal file
View File

@ -0,0 +1,77 @@
/******************** (C) COPYRIGHT 2022 Geek************************************
* File Name : Filter.c
* Current Version : V2.0
* Author : logzhan
* Date of Issued : 2022.10.15
* Comments : PDR滤波器支持库
********************************************************************************/
/* Header File Including ------------------------------------------------------*/
#include "stdio.h"
#include "Filter.h"
/* Function Definition --------------------------------------------------------*/
/**---------------------------------------------------------------------
* Function : FilterReset
* Description : 重置滤波器
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void FilterReset(Filter_t *f) {
f->reset = 1;
}
/**---------------------------------------------------------------------
* Function : FilterSetCoef
* Description : 滤波器设置参数
* Input : order coefficient array length, not filter order
* *b b coefficient
* *a a coefficient
* H0 dc signal gain
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void FilterSetCoef(Filter_t *f, int order, double *b, double *a, double H0) {
for (int i = 0; i < order; ++i) {
f->b[i] = b[i];
f->a[i] = a[i];
}
f->order = order;
f->H0 = H0;
f->reset = 1;
}
/**---------------------------------------------------------------------
* Function : FilterFilter
* Description : 滤波器滤波计算
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
double FilterFilter(Filter_t *f, double x) {
if (f->order < 1) {
printf("fileter order is set to incorrect order, pls chk \
your code!\n");
return 0;
}
double y = 0.0;
if (f->reset) {
for (int i = 0; i < f->order; ++i) {
f->yout[i] = f->H0 * x;
f->xin[i] = x;
}
y = f->yout[f->order - 1];
f->reset = 0;
}
// push x and y
for (int i = 0; i < f->order - 1; ++i) {
f->xin[i] = f->xin[i + 1];
f->yout[i] = f->yout[i + 1];
}
f->xin[f->order - 1] = x;
// filter
y = f->b[0] * f->xin[f->order - 1];
for (int i = 1; i < f->order; ++i) {
y += f->b[i] * f->xin[f->order - 1 - i] - f->a[i] * f->yout[f->order - 1 - i];
}
f->yout[f->order - 1] = y;
return y / f->H0;
}

211
1.Software/PDR 1.06/src/Interface.c Normal file
View File

@ -0,0 +1,211 @@
/******************** (C) COPYRIGHT 2022 Geek************************************
* File Name : Interface.c
* Current Version : V2.0
* Author : logzhan
* Date of Issued : 2022.10.15
* Comments : PDR导航算法对外部接口
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <stdio.h>
#include <math.h>
#include <string.h>
#include "pdr_api.h"
#include "PDRBase.h"
#include "Pedometer.h"
#include "Location.h"
#include "parseData.h"
#include "scene_recognition.h"
/* Global Variable Definition ------------------------------------------------*/
const char* Pedometer_Version = "2.0";
Sensor_t SensorDataHist[IMU_LAST_COUNT] = {{0}};
LatLng_t llaLast = { 0 };
Nmea_t Nmea;
IMU_t Imu;
/* Extern Variable Definition ------------------------------------------------*/
extern EKFPara_t EkfPara;
extern AHRS_t Ahrs;
extern GNSS_t pgnss;
/* Function Declaration ------------------------------------------------------*/
/**---------------------------------------------------------------------
* Function : RMC_Init
* Description : 初始化RMC协议
* Date : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
void RMC_Init(NmeaRMC_t * rmc)
{
rmc->type = SENSOR_MIN;
rmc->rmc_utc = ITEM_INVALID;
rmc->status = POSITIONING_N;
rmc->latitude = ITEM_INVALID;
rmc->rmc_latitude_ns = LATITUDE_N;
rmc->longitudinal = ITEM_INVALID;
rmc->longitudinal_ew = LATITUDE_N;
rmc->speed = ITEM_INVALID;
rmc->cog = ITEM_INVALID;
rmc->utc_ddmmyy = ITEM_INVALID;
rmc->magnetic = ITEM_INVALID;
rmc->mag_h = MAG_E;
rmc->mode = MODE_A;
rmc->time = ITEM_INVALID;
rmc->update = UN_UPDATE;
memset(rmc->rmc_check_sum, 0, sizeof(rmc->rmc_check_sum));
}
/**---------------------------------------------------------------------
* Function : NmeaFlag_Init
* Description : 初始化GPS的NMEA协议的相关标志位
* Date : 2020/7/3 logzhan
*---------------------------------------------------------------------**/
void NmeaFlag_Init(Nmea_t * ln)
{
ln->gga.update = 0;
for (char i = 0; i < 3; i++) {
ln->gsa[i].update = UN_UPDATE;
ln->gsv[i].update = UN_UPDATE;
}
ln->minTime = 0;
ln->maxTime = 0;
ln->update = UN_UPDATE;
RMC_Init(&ln->rmc[0]);
RMC_Init(&ln->rmc[1]);
ln->accuracy.time = ITEM_INVALID;
ln->accuracy.update = UN_UPDATE;
ln->accuracy.status = ITEM_INVALID;
ln->accuracy.err = ITEM_INVALID;
}
/**---------------------------------------------------------------------
* Function : clearNmeaFlg
* Description : 清空Nmea标志位
* Date : 2020/7/4 logzhan
*---------------------------------------------------------------------**/
void ClearNmeaFlg(Nmea_t * ln){
NmeaFlag_Init(ln);
}
/**---------------------------------------------------------------------
* Function : Algorithm_Init
* Description : PDR算法主初始化流程，包括PDR导航初始化、计步器状态初始化以及轨
* 迹平滑窗口的初始化
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void Algorithm_Init(void) {
// 导航相关初始化
PDR_Init();
// 计步器初始化
Pedometer_Init();
// 初始化sensor历史缓存
memset(SensorDataHist, 0, sizeof(SensorDataHist));
memset(&Nmea, 0, sizeof(Nmea));
memset(&Imu, 0, sizeof(Imu));
}
/**---------------------------------------------------------------------
* Function : ParseDataAndUpdate
* Description : 解析字符串信息并更新
* Date : 2022/10/16 logzhan
*---------------------------------------------------------------------**/
int ParseDataAndUpdate(char* line, LctFs_t* LocFusion)
{
ParseGnssInsData(line, &Imu, &Nmea);
// 给定Imu和Nmea结构体，返回融合位置
return PDRLocationMainLoop(&Imu, &Nmea, LocFusion);
}
/**---------------------------------------------------------------------
* Function : PDRLocationMainLoop
* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置
* 迹平滑窗口的初始化
* Date : 2020/07/03 logzhan
* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑
*---------------------------------------------------------------------**/
int PDRLocationMainLoop(IMU_t *imu, Nmea_t *nmea, LctFs_t *LocFusion) {
int type = 0;
if (imu->gyr.update) {
// 如果陀螺仪更新则采用惯性传感器计算
InsLocationUpdate(imu,&EkfPara);
imu->gyr.update = UN_UPDATE;
}
// 如果GPS不更新，返回
if (!nmea->update)return TYPE_FIX_NONE;
// 写GPS相关LOG信息
SaveGnssInfo(nmea, LocFusion, NULL);
// 有GPS则采用GPS融合定位
int flag = GnssInsFusionLocation(nmea, &EkfPara, LocFusion);
if (flag != TYPE_FIX_NONE) {
// 如果是开车这种，直接输出GPS，不进行平滑处理
LocFusion->latitude = R2D(LocFusion->latitude);
LocFusion->longitudinal = R2D(LocFusion->longitudinal);
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(nmea);
return type;
}
/**---------------------------------------------------------------------
* Function : SaveGnssInfo
* Description : 保存GPS相关信息
* Date : 2022/10/16 logzhan
*---------------------------------------------------------------------**/
void SaveGnssInfo(Nmea_t* nmea, LctFs_t* result, FILE* fp_gps)
{
NmeaRMC_t rmc = nmea->rmc[1];
// 选取更新时间的rmc
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->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,
rmc.speed);
WriteCsvStr(fp_gps, str);
result->gpsLat = rmc.latitude;
result->gpsLon = rmc.longitudinal;
}
}
/**----------------------------------------------------------------------
* Function : GetPDRVersion
* Description : 获取pdr版本号
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
const char* GetPDRVersion(void){
return Pedometer_Version;
}
/**----------------------------------------------------------------------
* Function : pdr_closeAlgorithm
* Description : 关闭PDR算法,释放资源
* Date : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
void Algorithm_DeInit() {
scene_recognition_destroy();
}

267
1.Software/PDR 1.06/src/Kalman.c Normal file
View File

@ -0,0 +1,267 @@
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : Kalman.c
* Current Version : V2.0
* Author : logzhan
* Date of Issued : 2022.10.15
* Comments : PDR扩展卡尔曼滤波器融合
********************************************************************************/
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <malloc.h>
#include "PDRBase.h"
#include "Kalman.h"
#include "Location.h"
#include "Matrix.h"
#include "Utils.h"
// 扩展卡尔曼向量缓存
static double EkfVecBuf[5][N] = { {0.0} };
// 扩展卡尔曼矩阵缓存
static double EkfMatBuf[7][N][N] = { { {0.0} } };
/**----------------------------------------------------------------------
* Function : KalmanFilter_Init
* Description : 初始化卡尔曼滤波器相关
* Date : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
void EKF_Init(void)
{
memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
memset(EkfVecBuf, 0, sizeof(double) * 5 * N);
}
/**----------------------------------------------------------------------
* Function : EKFStatePredict
* Description : pdr卡尔曼滤波器的状态预测方程
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void EKFStatePredict(EKFPara_t *ekf, double stepLen, PDR_t* pdr, int step) {
double angle = pdr->deltaHeading + ekf->pXk[3];
if(step == 0){
ekf->pXk[3] = angle;
return;
}
double(*Phi)[N] = (double(*)[N]) & (EkfMatBuf[0][0][0]);
double(*pvec1)[N] = (double(*)[N]) & (EkfMatBuf[1][0][0]);
double(*pvec2)[N] = (double(*)[N]) & (EkfMatBuf[2][0][0]);
memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
// xk+1 = xk + step * cos(angle) PDR位置更新方程
// yk+1 = yk + step * sin(angle) PDR位置更新方程
// sk+1 = sk 步长和上一次相同
// ak+1 = ak * (angle / ak) 或 ak+1 = ak
Phi[0][0] = 1; Phi[1][0] = 0; Phi[2][0] = 0; Phi[3][0] = 0;
Phi[0][1] = 0; Phi[1][1] = 1; Phi[2][1] = 0; Phi[3][1] = 0;
Phi[0][2] = cos(angle); Phi[1][2] = sin(angle); Phi[2][2] = 1; Phi[3][2] = 0;
Phi[0][3] = 0; Phi[1][3] = 0; Phi[2][3] = 0; Phi[3][3] = 1;
for (uchar i = 0; i < step; i++) {
// logzhan 21/02/06:
// phi[3][3]是为让kf的预测每次都是最新更新值, 如果phi[3][3]每次都是1，可能在惯性
// 导航更新过程中，偏航角的预测是固定的，实时性没有那么好，下面if判断可以在一定程度
// 让预测的值更新，但是效果暂时没有感觉出明显的好
if (fabs(ekf->pXk[3]) > 0.000001 &&
fabs(angle / ekf->pXk[3]) < 3)
{
Phi[3][3] = angle / ekf->pXk[3];
}
// 卡尔曼状态更新方程
// pXk = phi * pXk
VecMatMultiply(ekf->pXk, Phi, ekf->pXk);
// 卡尔曼更新协方差矩阵 : Pk = FPFt + Q
MatrixMultiply(Phi, ekf->pPk, pvec1); // F*P
MatrixTrans(Phi, pvec2); // Ft
MatrixMultiply(pvec1, pvec2, pvec1); // F*P*Ft
MatrixAdd(pvec1, ekf->Q, ekf->pPk); // F*P*Ft + Q
}
}
/**----------------------------------------------------------------------
* Function : EKFStateUpdate
* Description : 扩展卡尔曼滤波器的状态更新方程
* Date : 2020/7/22 logzhan
*---------------------------------------------------------------------**/
void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss, Classifer_t *sys, PDR_t *pdr,
int scene_type) {
double lambda = 0;
// NxN矩阵定义
double(*H)[N] = (double(*)[N])&(EkfMatBuf[0][0][0]);
double(*I)[N] = (double(*)[N])&(EkfMatBuf[1][0][0]);
double(*pvec1)[N] = (double(*)[N])&(EkfMatBuf[2][0][0]);
double(*Ht)[N] = (double(*)[N])&(EkfMatBuf[3][0][0]);
double(*pvec3)[N] = (double(*)[N])&(EkfMatBuf[4][0][0]);
double(*pvec4)[N] = (double(*)[N])&(EkfMatBuf[5][0][0]);
double(*pv3)[N] = (double(*)[N])&(EkfMatBuf[6][0][0]);
memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
memset(EkfVecBuf, 0, sizeof(double) * 5 * N);
// Nx1向量定义
double(*pvec5) = &(EkfVecBuf[1][0]);
double(*pvec6) = &(EkfVecBuf[2][0]);
double(*pv1) = &(EkfVecBuf[3][0]);
double(*pv2) = &(EkfVecBuf[4][0]);
// 创建单位矩阵
for (char i = 0; i < 4; i++) {
H[i][i] = 1;
I[i][i] = 1;
}
double* z = &(EkfVecBuf[0][0]);
// 获取观测向量
z[0] = pgnss->xNed; // 北向位置
z[1] = pgnss->yNed; // 东向位置
z[2] = 0.5; // 步长默认0.5
// 这个if条件实际是使用GPS的位置变化量和步数变化量估计步长, 这部分代码还没
// 调好，实际类似于步长不变
//if (pgnss->error > 0 && pgnss->error < 3.8 && pgnss->HDOP <= 0.4 &&
// g_pdr->deltaStepsPerGps != 0 && g_pdr->deltaStepsPerGps < 5) {
// // 利用GPS速度和步频估计步长
// //if (g_pdr->motionFreq > 2.5 && (sys->type == 1)) {
// //
// // //printf("z[2] = %f\n", z[2]);
// //}
// z[2] = pgnss->vel * 0.514 / (g_pdr->deltaStepsPerGps);
//}else {
// z[2] = kf->pXk[2];
//}
z[3] = pgnss->yaw * PI / 180; // 观测的航向角为GPS的航向角
// 计算 K = K = PHt / (R + H*P*H^T)
MatrixTrans(H, Ht); // 计算Ht
MatrixMultiply(H, kf->pPk, pvec1); // 计算HP
MatrixMultiply(pvec1, Ht, pvec3); // pvec3 = H*P*H^T
MatrixAdd(pvec3, kf->R, pvec4); // pvec4 = R + H*P*H^T
MatrixInverse(pvec4, pvec1); // 计算 1 / (R + H*P*H^T)
for (char i = 0; i < N; i++) {
for (uchar j = 0; j < N; j++) {
pv3[i][j] = pvec3[i][j];
}
}
MatrixMultiply(kf->pPk, Ht, pvec3); // 计算PHt
MatrixMultiply(pvec3, pvec1, kf->Kk); // 计算增益K = PHt / (R + H*P*H^T)
VecMatMultiply(kf->pXk, H, pvec5); // pvec5 = Hx'
VectorSub(z, pvec5, pvec6); // pvec6 = Z - Hx'
modAngle(&pvec6[3], -PI, PI);
for (char i = 0; i < N; i++) {
pv1[i] = pvec6[i];
}
VecMatMultiply(pvec6, kf->Kk, pvec5); // pvec5 = K*( z - Hx')
// Get the optimally updated state estimation
VectorAdd(kf->pXk, pvec5, kf->Xk);
// ---- prepare the matrix for updated estimate covariance ------
// pvec1 = K*H
MatrixMultiply(kf->Kk, H, pvec1);
// pvec2 = (I - K*H)
MatrixSub(I, pvec1, Ht);
// pvec3 = (I - K*H)*P
MatrixMultiply(Ht, kf->pPk, pvec3);
// pvec4 = (I- K*H)^T
MatrixTrans(Ht, pvec4);
// pvec1 = (I - K*H)*P*(I- K*H)^T
MatrixMultiply(pvec3, pvec4, pvec1);
// pvec2 = K*R
MatrixMultiply(kf->Kk, kf->R, Ht);
// pvec3 = K^T
MatrixTrans(kf->Kk, pvec3);
// pvec4 = K*R*K^T
MatrixMultiply(Ht, pvec3, pvec4);
// Get the updated estimate covariance: P' = (I - K*H)*P*(I- K*H)^T + K*R*K^T
MatrixAdd(pvec1, pvec4, kf->Pk);
// pv2 = (z - Hx')*( H*P*H^T )
VecMatMultiply(pv1, pv3, pv2);
// inner product the pre-fit residual and the transitioned residual: lamda = (z - Hx')*( H*P*H^T ) .* (z-Hx')
// Calculate the difference between original differenc and the transitioned,
// if the value is close to zero, it shows that there is a big gap in the
// oprimization. Thus, we believe our PDR algorithm
for (char i = 0; i < N; i++) {
lambda += pv2[i] * pv1[i];
}
kf->Lambda = lambda;
kf->pLat = pgnss->lat;
kf->pLon = pgnss->lon;
if (lambda >= 200)return;
for (char i = 0; i < N; i++) {
kf->Xk[i] = kf->pXk[i];
for (int j = 0; j < N; j++) {
kf->Pk[i][j] = kf->pPk[i][j];
}
}
}
/**----------------------------------------------------------------------
* Function : EKFCalQRMatrix
* Description : 根据GPS信号特征调整卡尔曼滤波噪声矩阵，q矩阵是过程噪声矩阵，
* 需要跟惯导预测位置精度相关。r矩阵为GNSS观测噪声，跟GPS输出的
* 信息精度有关。
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
void EKFCalQRMatrix(Nmea_t *nmea, PDR_t *pdr, Classifer_t *sys,
EKFPara_t *kf) {
if (pdr->MotionType != PDR_MOTION_TYPE_HANDHELD &&
pdr->MotionType != PDR_MOTION_TYPE_STATIC) {
kf->Q[0][0] = 25; // PDR预测位置噪声
kf->Q[1][1] = 25; // PDR预测位置噪声
kf->Q[2][2] = 1; // PDR步长噪声
kf->Q[3][3] = 100; // PDR方向噪声
kf->R[0][0] = 50; // GPS观测位置噪声
kf->R[1][1] = 50; // GPS观测位置噪声
kf->R[2][2] = 0.1; // GPS步长噪声
kf->R[3][3] = 50; // GPS方向噪声
return;
}
kf->Q[0][0] = 0.1; // PDR预测位置噪声
kf->Q[1][1] = 0.1; // PDR预测位置噪声
kf->Q[2][2] = 1; // PDR步长噪声
kf->Q[3][3] = 1; // PDR方向噪声
kf->R[0][0] = 100; // GPS预测位置噪声
kf->R[1][1] = 100; // GPS预测位置噪声
kf->R[2][2] = 0.1; // GPS步长噪声
kf->R[3][3] = 1000; // GPS方向噪声
if (nmea->accuracy.err > 0 &&
nmea->accuracy.err < 3.80 &&
nmea->gga.hdop <= 0.4)
{
kf->R[0][0] = 50; // GPS观测位置噪声
kf->R[1][1] = 50; // GPS观测位置噪声
kf->R[2][2] = 0.1; // GPS步长噪声
kf->R[3][3] = 50; // GPS方向噪声
}
if (fabs(R2D((pdr->GpsHeading - pdr->LastGpsHeading))) > 50 ||
pdr->GpsSpeed < 0.7 || pdr->GpsHeading == -1) {
kf->R[3][3] = 10000;
}
}

149
1.Software/PDR 1.06/src/LinearFit.c Normal file
View File

@ -0,0 +1,149 @@
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : pdr_linearFit.c
* Department : Sensor Algorithm Team
* Current Version : V2.0(compare QCOM SAP 5.0)
* Author : logzhan
* Date of Issued : 2020.7.4
* Comments : PDR 线性拟合相关函数支持, 主要是用于轨迹平滑
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <math.h>
#include <stdlib.h>
#include "LinearFit.h"
#include "Utils.h"
/**----------------------------------------------------------------------
* Function : pdr_linearLeastSquaresFitting
* Description : 线性最小二乘拟合 Y = c0 + c1 x
目标函数为sum| nDataY - (a*nDataX+b)|^2最小
* Date : 2020/7/4 logzhan
*---------------------------------------------------------------------**/
int linearLeastSquaresFit(const double *nDataX, const double *nDataY, const int nLength, double *a, double *b)
{
int fittingRes = 0;
if (nDataX == NULL || nDataY == NULL || a == NULL || b == NULL){
fittingRes = -1;
} else if (nLength <= 0){
fittingRes = 1;
} else {
fittingRes = gslFitLinear(nDataX, 1, nDataY, 1, nLength, b, a, NULL, NULL, NULL, NULL);
}
return fittingRes;
}
/**----------------------------------------------------------------------
* Function : gsl_fit_linear
* Description : Fit the data (x_i, y_i) to the linear relationship
Y = c0 + c1 x
returning,
c0, c1 -- coefficients
cov00, cov01, cov11 -- variance-covariance matrix of
c0 and c1,
sumsq -- sum of squares of residuals
This fit can be used in the case where the errors for
the data are uknown, but assumed equal for all points.
The resulting variance-covariance matrix estimates the
error in the coefficientsfrom the observed variance of
the points around the best fit line.
* Date :
*---------------------------------------------------------------------**/
static int gslFitLinear(const double *x, const size_t xstride,
const double *y, const size_t ystride,
const size_t n,
double *c0, double *c1,
double *cov_00, double *cov_01, double *cov_11, double *sumsq)
{
double m_x = 0, m_y = 0, m_dx2 = 0, m_dxdy = 0;
double dx;
double dy;
size_t i;
double s2 = 0, d2 = 0;
double b = 0;
double a = 0;
double d = 0;
for (i = 0; i < n; i++)
{
m_x += (x[i * xstride] - m_x) / (i + 1.0);
m_y += (y[i * ystride] - m_y) / (i + 1.0);
}
for (i = 0; i < n; i++)
{
dx = x[i * xstride] - m_x;
dy = y[i * ystride] - m_y;
m_dx2 += (dx * dx - m_dx2) / (i + 1.0);
m_dxdy += (dx * dy - m_dxdy) / (i + 1.0);
}
/* In terms of y = a + b x */
if (fabs(m_dx2) < 1e-20)return -1;
b = m_dxdy / m_dx2;
a = m_y - m_x * b;
*c0 = a;
*c1 = b;
/* Compute chi^2 = \sum (y_i - (a + b * x_i))^2 */
for (i = 0; i < n; i++)
{
dx = x[i * xstride] - m_x;
dy = y[i * ystride] - m_y;
d = dy - b * dx;
d2 += d * d;
}
s2 = d2 / (n - 2.0); /* chisq per degree of freedom */
/* 为函数增加空指针检查以使函数可以将一下参数设置为空指针 */
if (cov_00 != NULL)
*cov_00 = s2 * (1.0 / n) * (1 + m_x * m_x / m_dx2);
if (cov_11 != NULL)
*cov_11 = s2 * 1.0 / (n * m_dx2);
if (cov_01 != NULL)
*cov_01 = s2 * (-m_x) / (n * m_dx2);
if (sumsq != NULL)
*sumsq = d2;
return 0;
}
/**----------------------------------------------------------------------
* Function : leastDistanceLinearFit
* Description : 二维坐标系点的线性拟合，ax+by+c=0, 目标函数为点到拟合直线的距离
平方和最小， 这个函数的计算量还是相对较大的，主要注意这个函数不
适合频繁调用
* Input : x， y，二维坐标X和Y
num, 二维点个数
* Output : para， 拟合直线参数, para[0]:a, para[0]:b, para[0]:c
*---------------------------------------------------------------------**/
int leastDistanceLinearFit(double x[], double y[], int num, double para[])
{
int i;
double covMatrix[2][2] = { { 0.0 } };
double p[2][2] = { { 0.0 } };
if (num < 2)
return -1;
GetCovMatrix(x, y, covMatrix, num);
Jacobi(covMatrix, p, 2, 1e-10, 1000);
if (covMatrix[0][0] > covMatrix[1][1]) {
para[0] = p[0][1];
para[1] = p[1][1];
}
else {
para[0] = p[0][0];
para[1] = p[1][0];
}
para[2] = 0;
for (i = num - 1; i >= 0; i--) {
para[2] -= (para[0] * x[i] + para[1] * y[i]) / num;
}
return 0;
}

564
1.Software/PDR 1.06/src/Location.c Normal file
View File

@ -0,0 +1,564 @@
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : pdr_location.c
* Current Version : V1.2
* Author : logzhan
* Date of Issued : 2020.7.3
* Comments : PDR导航算法主流程
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <malloc.h>
#include <errno.h>
#include <stdlib.h>
#include <stdbool.h>
#include "Location.h"
#include "Kalman.h"
#include "Pedometer.h"
#include "AHRS.h"
#include "pdr_detector.h"
#include "Utils.h"
#include "scene_recognition.h"
#include "CoordTrans.h"
/* Global Variable Definition ------------------------------------------------*/
PDR_t pdr; // PDR全局信息
StepPredict stepPredict = { 0 };
Classifer_t sys;
GNSS_t pgnss; // 定位相关信息
EKFPara_t EkfPara;
double GpsPos[HIST_GPS_NUM][3] = {{0}};
static uint32_t g_status;
extern AHRS_t Ahrs;
double refGpsYaw = -1000;
double yaw_bias = -1;
float dir[2] = { 0 };
float bias_dir[2] = { 0 };
/**---------------------------------------------------------------------
* Function : PDRNav_Init
* Description : PDR导航系统初始化
* Date : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void PDR_Init(void) {
memset(&EkfPara, 0, sizeof(EkfPara));
memset(&pgnss, 0, sizeof(pgnss));
memset(GpsPos, 0, sizeof(GpsPos));
memset(&stepPredict, 0, sizeof(stepPredict));
memset(&sys, 0, sizeof(sys));
memset(dir, 0, sizeof(dir));
memset(bias_dir, 0, sizeof(bias_dir));
// GNSS定位初始化
InitGnssInfo();
// 初始化AHRS姿态解算相关
AHRS_Init();
// 检测器初始化
Detector_Init();
// PDR初始化
PDRInfoInit();
//base文件中姿态角计算初始化
EKF_Init();
// 场景识别模块初始化
SceneRecog_Init();
yaw_bias = -1;
}
void PDRInfoInit()
{
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.pcaAccuracy = ITEM_INVALID;
pdr.biasAccuracy = ITEM_INVALID;
pdr.sysStatus = IS_INITIAL;
pdr.sceneType = UN_OPEN_AREA;
pdr.fusionPdrFlg = PDR_FALSE;
pdr.trackHeading = ITEM_INVALID;
pdr.motionFreq = 0.1f;
memset(pdr.PllaInit, 0, sizeof(pdr.PllaInit));
}
/**---------------------------------------------------------------------
* Function : InitGnssInfo
* Description : PDS GNSS定位信息初始化
* Date : 2020/02/01 logzhan:
* 创建初始版本
* 2020/02/08: logzhan 修改initGps为initGnssInfo，更
* 加符合函数功能
*---------------------------------------------------------------------**/
void InitGnssInfo(void) {
pgnss.lastError = ACCURACY_ERR_MAX;
pgnss.overVelCount = 0;
}
/**---------------------------------------------------------------------
* Function : ResetSystemStatus
* Description : PDR 重设系统状态
* Date : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
void ResetSystemStatus(EKFPara_t *kf)
{
memset(kf, 0, sizeof(EKFPara_t));
pdr.fusionPdrFlg = PDR_FALSE;
pdr.sysStatus = IS_INITIAL;
pgnss.overVelCount = 0;
}
/**---------------------------------------------------------------------
* Function : InsLocationUpdate
* Description : PDR 惯性导航定位
* Date : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
int InsLocationUpdate(IMU_t* ImuData, EKFPara_t* Ekf) {
// AHRS姿态更新占2%的运行时间
if (UpdateAHRS(ImuData)) {
// 完成姿态估计后计算PCA， 占26%的运行时间
//ComputePCA(&g_ahrs);
}
// 更新用户运动类型检测器IMU信息以及用户运动类型检测, 占21%的运行时间
DetectorUpdateIMU(ImuData, &pdr);
pdr.gyroTime = ImuData->gyr.time;
// 为什么手持类型是0.1的运动频率？
if (pdr.MotionType == DETECTOR_TYPE_HANDHELD)pdr.motionFreq = 0.1f;
// 计步器输入传感器数据，占10%的运行时间
Pedometer_Update(ImuData, &pdr.steps);
// ----- Filter Initialization ------- //
if (pdr.sysStatus == IS_INITIAL) {
pdr.ts = ImuData->gyr.time;
pdr.lastSteps = pdr.steps;
stepPredict.lastTime = 0;
stepPredict.fnum = 0;
stepPredict.fsum = 0;
return 0;
}
pdr.ts = ImuData->gyr.time;
if ((0 == ImuData->acc.s[0] && 0 == ImuData->acc.s[1] && 0 == ImuData->acc.s[2]) ||
(0 == ImuData->mag.s[0] && 0 == ImuData->mag.s[1] && 0 == ImuData->mag.s[2])) {
return 0;
}
// 识别当前状态是否平稳，这个函数计算量到达整个系统10%的计算量, 以后根据情况进行优化
StateRecognition(ImuData->acc.s, &sys);
//pdr.heading = CalPredHeading(...);
//if (pdr.MotionType == PDR_MOTION_TYPE_HANDHELD)
{
double imuHeading = -Ahrs.Yaw;
if (imuHeading < 0) {
imuHeading += 360;
}
pdr.heading = D2R(imuHeading);
if (fabs(pdr.insHeadingOffset) > 0.000001) {
imuHeading = imuHeading - pdr.insHeadingOffset * (180 / 3.14159265);
pdr.heading = imuHeading / (180 / 3.14159265);
}
pdr.deltaHeading = pdr.heading - pdr.lastHeading;
}
// 角度增量约束(100Hz * 5.5° = 550°/sec)
if (pdr.deltaHeading > D2R(5.5)) {
pdr.deltaHeading = 0.0f;
}
// PDR 惯性位置预测
InsLocationPredict(&pdr, &stepPredict, ImuData, &pgnss, Ekf);
pdr.lastSteps = pdr.steps;
pdr.lastHeading = pdr.heading;
return 0;
}
/**----------------------------------------------------------------------
* Function : GnssInsFusionLocation
* Description : PDR GPS融合INS惯性定位
* Date : 2021/01/29 logzhan
*---------------------------------------------------------------------**/
int GnssInsFusionLocation(Nmea_t *nmea, EKFPara_t *kf, LctFs_t *result)
{
Nmea2Gnss(nmea, &pgnss);
// 检查时间差是否满足条件
if (fabs(pgnss.timestamps - pdr.ts) >= 1000)return TYPE_FIX_NONE;
// 根据nmea数据识别场景是否是开阔天空场景
pdr.sceneType = isOpenArea(nmea);
// 根据HDOP、速度、卫星数量等判断gps yaw是否可用
pdr.GpsSpeed = pgnss.vel * GPS_SPEED_UNIT;
pdr.LastGpsHeading = pdr.GpsHeading;
pdr.GpsHeading = GnssCalHeading(&pgnss);
// 如果在win32平台下仿真调试,则添加下面代码
#ifdef _WIN32
result->pdrHeading = kf->pXk[3];
result->gpsHeading = pdr.GpsHeading;
result->hdop = pgnss.HDOP;
result->accuracy = nmea->accuracy.err;
result->gpsSpeed = pdr.GpsSpeed;
result->motionType = pdr.MotionType;
#endif
// 利用Nmea信息修正Ins航向角
CalPdrHeadingOffset(nmea, &pdr);
// 计算两次GPS时间之间，步数变化量
pdr.deltaStepsPerGps = pdr.steps - pdr.lastGpsSteps;
pdr.lastGpsSteps = pdr.steps;
int mode = DetectFixMode(&pgnss, kf, &pdr, result);
if (mode != TYPE_FIX_PDR)return mode;
//根据历史GPS拟合输出预估的GPS轨迹航向角
pdr.trackHeading = CalGnssTrackHeading(GpsPos, pgnss);
// 如果系统没有初始化，则先初始化
if (pdr.sysStatus == IS_INITIAL) {
// Gnss系统初始对准
return GnssInsInitialAlignment(&pgnss, kf, &pdr, result);
}
// 如果惯导没有达到输出条件，则不预测轨迹
if (!pdr.fusionPdrFlg)
{
return ResetOutputLoction(&pgnss, &pdr, kf, result);
}
// 根据GPS信息, 计算kf的过程噪声和观测噪声
EKFCalQRMatrix(nmea, &pdr, &sys, kf);
// 运行卡尔曼位置融合
if (pgnss.satellites_num > 4){
// 如果GPS航向角处于无效状态且速度也是无效状态
if (pgnss.yaw == INVAILD_GPS_YAW || pgnss.vel <= 0.0) {
//if(pdr.heading != INVAILD_GPS_YAW){
// pgnss.yaw = (float)(R2D(pdr.heading));
//}else if (pdr.trackHeading != INVAILD_GPS_YAW) {
// pgnss.yaw = (float)(R2D(pdr.trackHeading));
//}
pgnss.yaw = (float)kf->pXk[3];
}
if (pgnss.yaw != INVAILD_GPS_YAW) {
EkfGnssInsLocFusion(&pgnss, &pdr, &sys, yaw_bias, kf, result);
return TYPE_FIX_PDR;
}
}
// 不满足推算条件，直接输出原始GPS
if (pgnss.satellites_num > 4 && nmea->accuracy.err > 0 &&
nmea->accuracy.err < 15) {
OutputRawGnssPos(&pgnss, result);
pdr.NoGnssCount = 0;
return TYPE_FIX_GPS;
}
// 如果没有GPS信号，那么PDR也会接着推算位置, 但是最多推算10个点
if (pdr.NoGnssCount < MAX_NO_GPS_PREDICT) {
// 无GPS状态位置推算
NoGnssInfoPredict(kf, result, &pdr);
return TYPE_FIX_PDR;
}
pdr.NoGnssCount++;
return 0;
}
/**----------------------------------------------------------------------
* Function : NoGnssInfoPredict
* Description : 在没有gps信息时预测GPS位置
* Date : 2022/09/16
*---------------------------------------------------------------------**/
void NoGnssInfoPredict(EKFPara_t* kf, LctFs_t* result, PDR_t* p_pdr)
{
double ned[3] = {0}, lla[3] = {0};
ned[0] = kf->pXk[0] - p_pdr->x0;
ned[1] = kf->pXk[1] - p_pdr->y0;
ned[2] = 0;
NED2WGS84(p_pdr->PllaInit, ned, lla);
result->latitude = lla[0];
result->latitude_ns = pgnss.lat_ns;
result->longitudinal = lla[1];
result->longitudinal_ew = pgnss.lon_ew;
result->time = pgnss.timestamps;
p_pdr->NoGnssCount++;
}
/**----------------------------------------------------------------------
* Function : calPdrHeadingOffset
* Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移
* Date : 2020/07/08 logzhan
*---------------------------------------------------------------------**/
void CalPdrHeadingOffset(Nmea_t* nmea_data, PDR_t* p_pdr) {
//惯导姿态角与GPS行进角度的对准（判断GPS信号是否正常以及惯导是否平稳）
if (refGpsYaw != -1000)return;
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))
{
/*这里放姿态角与GPS对准part1 start*/
double imuHeading = atan2(2 * (Ahrs.q[0] * Ahrs.q[3] + Ahrs.q[1] * Ahrs.q[2]),
1 - 2 * (Ahrs.q[2] * Ahrs.q[2] + Ahrs.q[3] * Ahrs.q[3]));
if (imuHeading < 0) {
imuHeading += TWO_PI;
}
if (fabs(imuHeading - p_pdr->GpsHeading) < PI / 18)
{
//printf("offset 标定\n");
//p_pdr->insHeadingOffset = imuHeading - p_pdr->gpsHeading;
}
}
}
/**----------------------------------------------------------------------
* Function : DetectFixMode
* Description : 检测当前PDR处于的模式，如果是车载和静止模式,根据情况选择输出
* 原始GPS或者不输出
* Date : 2020/07/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高
* 代码的可读性
*---------------------------------------------------------------------**/
int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* pdr, LctFs_t* result) {
// 检测用户是否处于静止状态
if (DetUserStatic(pdr, pgnss, pdr->deltaStepsPerGps)) {
if (pgnss->error > 0 && pgnss->error < 15 &&
pgnss->lastError >= pgnss->error) {
OutputRawGnssPos(pgnss, result);
pgnss->lastError = pgnss->error;
return TYPE_FIX_PDR;
}
// 否则不输出位置
return TYPE_FIX_NONE;
}
// 如果是检测到是开车状态则输出GPS位置
if (DetectCarMode(pgnss, pdr, pdr->deltaStepsPerGps, &pgnss->overVelCount)) {
// 如果检测到车载模式，则重置PDR状态，需要重新初始化
ResetSystemStatus(kf);
if (pgnss->error >= 0 || (pgnss->satellites_num > 4
&& pgnss->HDOP < 2.0)) {
OutputRawGnssPos(pgnss, result);
return TYPE_FIX_GPS;
}
// 精度过差，不输出位置
return TYPE_FIX_NONE;
}
return TYPE_FIX_PDR;
}
/**----------------------------------------------------------------------
* Function : GnssCalHeading
* Description : 获取GPS偏航角(0 - 360°)
* Date : 2022/09/16 logzhan
* 代码的可读性
*---------------------------------------------------------------------**/
double GnssCalHeading(GNSS_t* pgnss) {
double gnssHeading = INVAILD_GPS_YAW;
if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.0
&& pgnss->yaw != INVAILD_GPS_YAW && pgnss->error < 50) {
gnssHeading = pgnss->yaw * RADIAN_PER_DEG;
}
return gnssHeading;
}
/**----------------------------------------------------------------------
* Function : pdr_nmea2Gnss
* Description : nmea数据结构转gnss数据
* Date : 2020/07/08 logzhan
*---------------------------------------------------------------------**/
void Nmea2Gnss(Nmea_t* nmea_data, GNSS_t* pgnss)
{
NmeaRMC_t rmc = { 0 };
if (nmea_data->rmc[0].time > nmea_data->rmc[1].time) {
rmc = nmea_data->rmc[0];
}else {
rmc = nmea_data->rmc[1];
}
// 更新上一次的Accuracy Error
pgnss->lastError = pgnss->error;
pgnss->lat = rmc.latitude * PI / 180;
pgnss->lat_ns = rmc.rmc_latitude_ns;
pgnss->lon = rmc.longitudinal * PI / 180;
pgnss->lon_ew = rmc.longitudinal_ew;
//pgnss->vel = rmc.speed * GPS_SPEED_UNIT;
pgnss->vel = rmc.speed;
pgnss->HDOP = nmea_data->gsa[0].hdop;
pgnss->yaw = rmc.cog;
pgnss->quality = 0;
pgnss->error = nmea_data->accuracy.err;
pgnss->timestamps = rmc.time;
pgnss->satellites_num = nmea_data->gga.satellites_nb;
}
/**----------------------------------------------------------------------
* 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 : 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;
}

460
1.Software/PDR 1.06/src/Main.cpp Normal file
View File

@ -0,0 +1,460 @@
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : pdr_main.cpp
* Department : Sensor Algorithm Team
* Current Version : V2.0
* Author : logzhan
* Date of Issued : 2021.1.28
* Comments : PDR PC端仿真主文件
* Modify : 2021/02/04 logzhan : 优化kml显示，kml增加航向角图形
* 显示，hdop、heading、accuracy信息在Google Map的显示，方便调
* 试代码。
* 2021/02/18 logzhan : 增加kml的时间显示，能够拖动进
* 度条显示轨迹。可以对比查看轨迹效果。
*
* 一、优化方向和工作内容
* 1) GPS速度实际需要乘以0.51444的，但是除了滤波器计算步频之外，
* 其他地方都是用的实际的GPS速度，包括步数预测
* 2) 惯性导航更新部分buffer mean使用的太多了, 计算平均没有优化
* 时间分析发现，buffer mean耗费了大量时间
* 2021/02/10 logzhan:
* 1) 需要跟品质谈高通对比版本，防止最后效果和高通SAP5.0持平后，
* 高通出了新版本，品质不认可效果改进。
* 2) 目前GPS的速度是没有乘以0.51444的，许多地方计算没有统一，需
* 要统一。
* 3）惯性导航更新部分buffer mean使用的太多了, 计算平均没有优化
* 时间分析发现，buffer mean耗费了大量时间
* 4)*目前在不依赖磁力计的版本也能达到比以前有磁力计效果还好。但是
* 磁力计依然是一个重要的辅助确定方向的手段。所以利用GPS航向作为
* 观测，磁力计启动动态校准，校准完成后才启动辅助磁力计校准方向，
* 或者作为一个观测量，用于提升预测方向的准确程度。
* 5)*动态偏移对准，后续会开始开发PCA方向估计，那么PCA估计方向肯定
* 和真实方向存在一定的偏差，需要动态对准偏移。
* 6）目前的计步器，输出不够实时，对于步长的估计不够准确。目前测试
* 的情况是，加入动态步长估计后貌似轨迹会偏大，是不是计步偏少？
* 7）这个步频计算用到了fft，感觉没有太大作用，可以考虑在新版本中
* 去除，然后以后开发时如果需要可以参考原理重新优化。
* 8) 赖工提到路网信息，目前比较确定的是能拿到路的方向信息，考虑
* 在选定的场景标定一下经纬度和最近的道路方向信息，观察如何提升
* 定位精度(方向辅助，减少方向上的漂移)。
* 9) PDR 目前一定概率初始对准方向会偏，所以初始对准时，检测是否
* 是直线路径，然后考虑用轨迹计算平均方向角，辅助初始化。
* 10) 跟张经睿讨论过，场景识别模块以前主要是针对以前的单频手机，
* 所以这个模块考虑重构，因为以前单频手机的参数和现在双频手机不同。
* 需要重新优化。
* 11) 目前PDR算法在观测方向时，还是一定概率会被带偏，所以导致转弯
* 转向不够，然而又不够相信GPS方向，考虑通过多次GPS方向角方差不大时
* 调整GPS方向角的噪声策略。
* 12) kml支持时间信息，可以拖动进度条比较轨迹同时运行的情况 ---- logzhan finish
*
* 二、调整规划为：
第一期：手持稳定优化 + 摆手(平滑模块主要生效) + 多圈优化 => 不会比之前效果差，部分场景提升
第二期：手持稳定优化 + 摆手(PCA + 平滑) + 多圈 => 在摆手场景优化，覆盖一大半场景
第三期：手持稳定优化 + 摆手 + 任意动作 => 鲁棒性优化，覆盖全部使用场景
第四期：手持稳定优化 + 摆手 + 任意动作 + 多圈 + 真值仪 => 保证轨迹优化和方向优化条件下，精度提升，可以方便对外
********************************************************************************/
#include <stdio.h>
#include <string>
#include <errno.h>
#include <stdlib.h>
#include "pdr_api.h"
#include "Main.h"
#include "Utils.h"
#include "time.h"
using namespace std;
/* Global Variable Definition ------------------------------------------------*/
ResultTracks resTracks;
FILE* SimFile = NULL;
/* Extern Variable Definition ------------------------------------------------*/
extern "C" PDR pdr;
extern "C" double refGpsYaw;
/**----------------------------------------------------------------------
* Function : main
* Description : pdr算法仿真用的main函数
* Date : 2021/01/25 logzhan
*---------------------------------------------------------------------**/
int main(int argc, char* argv[])
{
string logPath = "E:\\SoftwareProject\\GnssIns\\GnssIns-PDR-Private\\1.Software\\PDR 1.06\\Release\\";
string logDate = "shenzhen";
string logLabel = "data\\";
string fileHead;
char line[256] = { 0 };
// 解析命令行, 用于解析参数
for (int i = 0; i < argc; i++) {
string s = argv[i];
if(i == 1)logPath = s;
if(i == 2)logDate = s;
}
clock_t t = clock();
// 获取kml输出路径
string kmlPath = logPath + logDate + "\\output\\";
// 仿真文件夹路径和Catalog.txt路径
string filePath = logPath + logDate + "\\" + logLabel + "\\";
string catalogPath = filePath + "catalog.txt";
FILE* catalogFp = fopen(catalogPath.c_str(), "r");
if (catalogFp == NULL){
printf("open catalog.txt file %s error %s\n", catalogPath.c_str(),
strerror(errno));
system("pause");
return -1;
}
while (!feof(catalogFp))
{
FILE* fileFp = getSimulateFile(catalogFp, filePath, fileHead);
// PDR 仿真流程
Algorithm_Init();
LctFs_t locFusion;
memset(&locFusion,0, sizeof(locFusion));
memset(&resTracks, 0, sizeof(resTracks));
// 解析文本仿真
while (fgets(line, 256, fileFp)){
// 解析文本数据仿真
if (ParseDataAndUpdate(line, &locFusion) != TYPE_FIX_NONE){
// 更新结果轨迹，用于最后的kml生成
UpdateResTrack(resTracks, locFusion);
}
}
// 关闭PDR算法，释放资源
Algorithm_DeInit();
// 写结果kml
KmlWrite(kmlPath, fileHead, string("_PDR_ZL_2.1c"));
// 关闭文件
fclose(fileFp);
}
// 打印仿真时间信息
printf("PDR simulate spends %f s\n", ((double)(clock() - t) / CLOCKS_PER_SEC));
system("pause");
return 0;
}
/**----------------------------------------------------------------------
* Function : getSimulateFile
* Description : 给定文件名，以及文件路径，获取仿真文件的文件指针
* catalogFp : 目录文件的文件指针
* path_file ：仿真文件的文件目录
* fileHead : 通过C++引用返回的去除了扩展名的文件名称
* Date : 2021/01/25 logzhan
*---------------------------------------------------------------------**/
FILE* getSimulateFile(FILE* catalogFp, string path_file, string& fileHead) {
char file_name[256] = { 0 };
FILE* fp = NULL;
if (fscanf(catalogFp, "%s\n", file_name) == -1){
printf("get data file name failed\n");
system("pause");
exit(-1);
}
string fileNamePath = path_file + string(file_name);
fp = fopen(fileNamePath.c_str(), "rb");
if (fp == NULL){
printf("open file %s failed error %s\n", fileNamePath.c_str(), strerror(errno));
system("pause");
exit(-1);
}
// 去除文件扩展名.csv
fileHead = file_name;
fileHead = fileHead.substr(0, fileHead.length() - 4);
printf("%s\n", fileNamePath.c_str());
return fp;
}
/**----------------------------------------------------------------------
* Function : gpsYaw2GoogleYaw
* Description : 为了让kml显示的角度方位正常，需要把0-360顺时针旋转的Yaw转换
* 为谷歌支持的Yaw规则
* Date : 2021/01/25 logzhan
*---------------------------------------------------------------------**/
double gpsYaw2GoogleYaw(double heading) {
double gh = R2D(heading) - 180;
while (gh > 360)gh -= 360;
while (gh < 0)gh += 360;
return gh;
}
/**----------------------------------------------------------------------
* Function : UpdateResTrack
* Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
void UpdateResTrack(ResultTracks& resTrack, LctFs_t& lctfs)
{
resTrack.pdrTrack[resTrack.pdrLen].lat = lctfs.latitude;
resTrack.pdrTrack[resTrack.pdrLen].lon = lctfs.longitudinal;
resTrack.pdrTrack[resTrack.pdrLen].heading =
gpsYaw2GoogleYaw(lctfs.pdrHeading);
resTrack.pdrTrack[resTrack.pdrLen].hdop = lctfs.hdop;
resTrack.pdrTrack[resTrack.pdrLen].accuracy = lctfs.accuracy;
resTrack.pdrTrack[resTrack.pdrLen].time = lctfs.time;
resTrack.pdrTrack[resTrack.pdrLen].motionType = lctfs.motionType;
resTrack.pdrLen++;
if (lctfs.gpsLat > 0 && lctfs.gpsLon > 0) {
resTrack.gpsTrack[resTrack.gpsLen].lat = lctfs.gpsLat;
resTrack.gpsTrack[resTrack.gpsLen].lon = lctfs.gpsLon;
resTrack.gpsTrack[resTrack.gpsLen].heading =
gpsYaw2GoogleYaw(lctfs.gpsHeading);
resTrack.gpsTrack[resTrack.gpsLen].vel = lctfs.gpsSpeed;
resTrack.gpsTrack[resTrack.gpsLen].hdop = lctfs.hdop;
resTrack.gpsTrack[resTrack.gpsLen].accuracy = lctfs.accuracy;
resTrack.gpsTrack[resTrack.gpsLen].time = lctfs.time;
resTrack.gpsLen++;
}
}
string time2str(double t) {
long time = (long)t;
if (t < 10) {
string str = "0" + std::to_string(time);
return str;
}else {
string str = std::to_string(time);
return str;
}
}
/**----------------------------------------------------------------------
* Function : KmlWrite
* Description : 将pdr算法输出的gps和pdr轨迹写为kml形式
* path : kml文件的输出文件路径
* name : kml文件主体名称
* postfix ：在主体名称后面添加的后缀，用于区分类型或者版本
* Date : 2020/11/1 logzhan
*---------------------------------------------------------------------**/
void KmlWrite(string path, string name, string postfix)
{
string pdrColor = "ff0000ff";
string gpsColor = "ff00ffff";
if (path == "") {
return;
}
string kmlPath = path + name + postfix + ".kml";
string kmlName = name + postfix;
FILE* fid = fopen(kmlPath.c_str(), "w");
fprintf(fid, "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n");
fprintf(fid, "<kml xmlns=\"http://www.opengis.net/kml/2.2\">\n");
fprintf(fid, "\t<Document>\n");
fprintf(fid, "\t\t<name>%s</name>\n", kmlName.c_str());
fprintf(fid, "\t\t<Folder id=\"ID1\">\n");
fprintf(fid, "\t\t<name>GPS Fixs</name>\n");
// 写gps结果
for (int i = 0; i < resTracks.gpsLen; i++) {
fprintf(fid, "\t\t<Placemark>\n");
fprintf(fid, "\t\t\t<Style>\n");
fprintf(fid, "\t\t\t\t<IconStyle>\n");
fprintf(fid, "\t\t\t\t\t<Icon>\n");
fprintf(fid, "\t\t\t\t\t\t<href>%s</href>\n",
"http://maps.google.com/mapfiles/kml/shapes/arrow.png");
fprintf(fid, "\t\t\t\t\t</Icon>\n");
fprintf(fid, "\t\t\t\t\t<scale>%.2f</scale>\n", 0.4);
// 描述轨迹方向信息
fprintf(fid, "\t\t\t\t\t<heading>%.7f</heading>\n", resTracks.gpsTrack[i].heading);
fprintf(fid, "\t\t\t\t\t<color>%s</color>\n", gpsColor.c_str());
fprintf(fid, "\t\t\t\t</IconStyle>\n");
fprintf(fid, "\t\t\t</Style>\n");
// 写入时间信息
double h, m, s;
pdr_utc2hms(resTracks.gpsTrack[i].time, &h, &m, &s);
fprintf(fid, "\t\t\t<TimeStamp>\n");
fprintf(fid, "\t\t\t\t<when>2021-01-22T%s:%s:%s</when>\n", time2str(h).c_str(),
time2str(m).c_str(), time2str(s).c_str());
fprintf(fid, "\t\t\t</TimeStamp>\n");
// 写入描述信息
fprintf(fid, "\t\t\t<description>\n");
fprintf(fid, "\t\t\t\t<![CDATA[\n");
fprintf(fid, "\t\t\t\t<dl>\n");
fprintf(fid, "\t\t\t\t<dd>East : 0.0 (m/s)</dd>\n");
fprintf(fid, "\t\t\t\t<dd>HDOP : %.2f (m/s)</dd>\n",
resTracks.gpsTrack[i].hdop);
fprintf(fid, "\t\t\t\t<dd>accuracy : %.2f (m)</dd>\n",
resTracks.gpsTrack[i].accuracy);
fprintf(fid, "\t\t\t\t<dd>speed : %.2f (m/s)</dd>\n",
resTracks.gpsTrack[i].vel);
fprintf(fid, "\t\t\t\t<dd>Heading : %.2f (degrees) </dd>\n",
resTracks.gpsTrack[i].heading);
fprintf(fid, "\t\t\t\t</dl><hr>]]>\n");
fprintf(fid, "\t\t\t</description>\n");
// 写入坐标信息
fprintf(fid, "\t\t\t<Point>\n");
fprintf(fid, "\t\t\t\t<coordinates>%.10f,%.10f</coordinates>\n",
resTracks.gpsTrack[i].lon,
resTracks.gpsTrack[i].lat);
fprintf(fid, "\t\t\t</Point>\n");
fprintf(fid, "\t\t</Placemark>\n");
}
fprintf(fid, "\t\t</Folder>\n");
fprintf(fid, "\t\t<Folder id=\"ID2\">\n");
fprintf(fid, "\t\t<name>PDR Fixs</name>\n");
// 写pdr结果
for (int i = 0; i < resTracks.pdrLen; i++) {
fprintf(fid, "\t\t<Placemark>\n");
fprintf(fid, "\t\t\t<Style>\n");
fprintf(fid, "\t\t\t\t<IconStyle>\n");
fprintf(fid, "\t\t\t\t\t<Icon>\n");
fprintf(fid, "\t\t\t\t\t\t<href>%s</href>\n",
"http://maps.google.com/mapfiles/kml/shapes/arrow.png");
fprintf(fid, "\t\t\t\t\t</Icon>\n");
fprintf(fid, "\t\t\t\t\t<scale>%.2f</scale>\n", 0.4);
fprintf(fid, "\t\t\t\t\t<heading>%.7f</heading>\n",
resTracks.pdrTrack[i].heading);
fprintf(fid, "\t\t\t\t\t<color>%s</color>\n", pdrColor.c_str());
fprintf(fid, "\t\t\t\t</IconStyle>\n");
fprintf(fid, "\t\t\t</Style>\n");
// 写入时间信息
double h, m, s;
pdr_utc2hms(resTracks.pdrTrack[i].time, &h, &m, &s);
fprintf(fid, "\t\t\t<TimeStamp>\n");
fprintf(fid, "\t\t\t\t<when>2021-01-22T%s:%s:%s</when>\n", time2str(h).c_str(),
time2str(m).c_str(), time2str(s).c_str());
fprintf(fid, "\t\t\t</TimeStamp>\n");
// description
fprintf(fid, "\t\t\t<description>\n");
fprintf(fid, "\t\t\t\t<![CDATA[\n");
fprintf(fid, "\t\t\t\t<dl>\n");
fprintf(fid, "\t\t\t\t<dd>East : 0.0 (m/s)</dd>\n");
fprintf(fid, "\t\t\t\t<dd>HDOP : %.2f (m/s)</dd>\n",
resTracks.pdrTrack[i].hdop);
fprintf(fid, "\t\t\t\t<dd>MOTION TYPE : %s (m/s)</dd>\n",
Motion2TypeStr(resTracks.pdrTrack[i].motionType));
fprintf(fid, "\t\t\t\t<dd>accuracy : %.2f (m/s)</dd>\n",
resTracks.pdrTrack[i].accuracy);
fprintf(fid, "\t\t\t\t<dd>Heading : %.2f (degrees) </dd>\n",
resTracks.pdrTrack[i].heading);
fprintf(fid, "\t\t\t\t</dl><hr>]]>\n");
fprintf(fid, "\t\t\t</description>\n");
fprintf(fid, "\t\t\t<Point>\n");
fprintf(fid, "\t\t\t\t<coordinates>%.10f,%.10f</coordinates>\n",
resTracks.pdrTrack[i].lon,
resTracks.pdrTrack[i].lat);
fprintf(fid, "\t\t\t</Point>\n");
fprintf(fid, "\t\t</Placemark>\n");
}
fprintf(fid, "\t\t</Folder>\n");
fprintf(fid, "\t</Document>\n");
fprintf(fid, "</kml>\n");
fclose(fid);
}
#ifdef _WIN32
/**----------------------------------------------------------------------
* Function : PDR_Init
* Description : 初始化PDR算法(DLL调用)
* Date : 2022/09/15 logzhan
*---------------------------------------------------------------------**/
void LibPDR_Init() {
Algorithm_Init();
}
/**----------------------------------------------------------------------
* Function : PDR_StepUpdateGPS
* Description : 仿真器按照GPS单步执行(DLL调用)
* Date : 2022/9/16 logzhan
*---------------------------------------------------------------------**/
PosFusion LibPDR_StepUpdateGPS(int useGpsFlg)
{
PosFusion fusion;
fusion.vaild = 0;
Nmea_t location_nmea;
Nmea_t* ln = &location_nmea;
imu imu_data;
LctFs_t lct_fusion;
memset(&lct_fusion, 0, sizeof(lct_fusion));
//memset(&location_nmea, 0, sizeof(location_nmea));
memset(&imu_data, 0, sizeof(imu_data));
/*memset(&kf, 0, sizeof(kf));*/
char line[256] = { 0 };
while (fgets(line, 256, SimFile)) {
int flag = ParseDataAndUpdate(line, &lct_fusion);
if (flag) {
fusion.gpsLat = lct_fusion.gpsLat;
fusion.gpsLon = lct_fusion.gpsLon;
}
// 写PDR融合结果
if (flag) {
fusion.lat = lct_fusion.latitude;
fusion.lon = lct_fusion.longitudinal;
fusion.t = lct_fusion.time;
fusion.vaild = 1;
break;
}
}
return fusion;
}
/**----------------------------------------------------------------------
* Function : setSimulatorFileCsvPath
* Description : 设置仿真文件的路径(DLL调用)
* Date : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
void setSimulatorFileCsvPath(char* path) {
if (SimFile == NULL) {
SimFile = fopen(path, "rb");
}
else {
fclose(SimFile);
SimFile = NULL;
}
}
/**----------------------------------------------------------------------
* Function : setRefGpsYaw
* Description : 设置GPS参考角度
* Date : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
void setRefGpsYaw() {
refGpsYaw = pdr.GpsHeading;
printf("ref Gps Yaw = %f\n", refGpsYaw);
//g_pdr.sysStatus = IS_INITIAL;
//g_pdr.fusionPdrFlg = ON;
}
/**----------------------------------------------------------------------
* Function : closePdrAlgo
* Description : 关闭pdr算法，释放相关内存，用于dll调用(DLL调用)
* Date : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
void LibPDR_DeInit() {
Algorithm_DeInit();
if (SimFile != NULL) {
fclose(SimFile);
SimFile = NULL;
}
}
#endif

195
1.Software/PDR 1.06/src/Matrix.c Normal file
View File

@ -0,0 +1,195 @@
/******************** (C) COPYRIGHT 2022 Geek************************************
* File Name : matrix.c
* Current Version : V2.0
* Author : logzhan
* Date of Issued : 2022.09.14
* Comments : PDR矩阵运算库支持
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include "Matrix.h"
/**---------------------------------------------------------------------
* Function : MatrixTrans
* Description : 矩阵转置
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void MatrixTrans(double a[N][N], double r[N][N]) {
int i, j;
for (i = 0; i < N; i++) {
for (j = 0; j < N; j++) {
r[i][j] = a[j][i];
}
}
}
/**---------------------------------------------------------------------
* Function : VecMatMultiply
* Description : 向量和矩阵相乘 r = b * a
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void VecMatMultiply(double a[N], double b[N][N], double r[N]) {
int i, j;
double temp[N] = { 0.0 };
//for (i = 0; i < N; i++) {
// temp[i] = a[i];
//}
for (i = 0; i < N; i++) {
for (j = 0; j < N; j++) {
temp[i] += ((b[i][j] * 100) * (a[j] * 100)) / 10000;
}
}
for (i = 0; i < N; i++) {
r[i] = temp[i];
}
}
/**---------------------------------------------------------------------
* Function : MatrixMultiply
* Description : 矩阵和矩阵相乘 r = a * b
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void MatrixMultiply(double a[N][N], double b[N][N], double r[N][N]) {
int i, j, m;
double temp[N][N] = { { 0.0 } };
for (i = 0; i < N; i++) {
for (j = 0; j < N; j++) {
for (m = 0; m < N; m++) {
//temp[i][j] += a[j][m] * b[m][j];
temp[i][j] += a[i][m] * b[m][j];
}
}
}
for (i = 0; i < N; i++) {
for (j = 0; j < N; j++) {
r[i][j] = temp[i][j];
}
}
}
/**---------------------------------------------------------------------
* Function : MatrixAdd
* Description : 矩阵和矩阵相加 r = a + b, 注意这个函数支持a = a + b
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void MatrixAdd(double a[N][N], double b[N][N], double r[N][N]) {
int i, j;
for (i = 0; i < N; i++) {
for (j = 0; j < N; j++) {
r[i][j] = a[i][j] + b[i][j];
}
}
}
/**---------------------------------------------------------------------
* Function : VectorAdd
* Description : 向量和向量相加 r = a + b, 注意这个函数支持a = a + b
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void VectorAdd(double a[N], double b[N], double r[N]) {
int i;
for (i = 0; i < N; i++) {
r[i] = a[i] + b[i];
}
}
/**---------------------------------------------------------------------
* Function : MatrixSub
* Description : 矩阵和矩阵相减 r = a - b, 注意这个函数支持a = a - b
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void MatrixSub(double a[N][N], double b[N][N], double r[N][N]) {
int i, j;
for (i = 0; i < N; i++) {
for (j = 0; j < N; j++) {
r[i][j] = a[i][j] - b[i][j];
}
}
}
/**---------------------------------------------------------------------
* Function : VectorSub
* Description : 向量和向量相减 r = a - b, 注意这个函数支持a = a - b
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void VectorSub(double a[N], double b[N], double r[N]) {
int i;
for (i = 0; i < N; i++) {
r[i] = a[i] - b[i];
}
}
/**---------------------------------------------------------------------
* Function : MatrixInverse
* Description : 求矩阵的逆矩阵
* Date : 2022/09/14 logzhan
*---------------------------------------------------------------------**/
void MatrixInverse(double(*a)[N], double (*a_inv)[N]) {
double l[N][N] = { { 0.0 } };
double u[N][N] = { { 0.0 } };
double l_inv[N][N] = { { 0.0 } };
double u_inv[N][N] = { { 0.0 } };
double temp[N][N] = { { 0.0 } };
int i, j, k;
double s;
for (i = 0; i < N; i++)l[i][i] = 1;
for (i = 0; i < N; i++){
for (j = i; j <N; j++){
s = 0;
for (k = 0; k < i; k++){
s += l[i][k] * u[k][j];
}
u[i][j] = a[i][j] - s;
}
for (j = i + 1; j < N; j++){
s = 0;
for (k = 0; k < i; k++){
s += l[j][k] * u[k][i];
}
l[j][i] = (a[j][i] - s) / u[i][i];
}
}
for (i = 0; i < N; i++)l_inv[i][i] = 1;
for (i = 1; i < N; i++){
for (j = 0; j < i; j++){
s = 0;
for (k = 0; k < i; k++){
s += l[i][k] * l_inv[k][j];
}
l_inv[i][j] = -s;
}
}
for (i = 0; i < N; i++){
u_inv[i][i] = 1 / u[i][i];
}
for (i = 1; i < N; i++)
{
for (j = i - 1; j >= 0; j--)
{
s = 0;
for (k = j + 1; k <= i; k++)
{
s += u[j][k] * u_inv[k][i];
}
u_inv[j][i] = -s / u[j][j];
}
}
for (i = 0; i < N; i++){
for (j = 0; j < N; j++){
for (k = 0; k < N; k++){
temp[i][j] += u_inv[i][k] * l_inv[k][j];
}
}
}
for (i = 0; i < N; i++) {
for (j = 0; j < N; j++) {
a_inv[i][j] = temp[i][j];
}
}
}

295
1.Software/PDR 1.06/src/Munkres.c Normal file
View File

@ -0,0 +1,295 @@
/* Header File Including ------------------------------------------------------------------------ */
#include <stdint.h>
#include <string.h>
#include "m_munkres.h"
/* Macro Definition ----------------------------------------------------------------------------- */
#define COST(i, j) cost[(i) + (j) * m]
#define MARK(i, j) mark[(i) + (j) * m]
#define PATH(i, j) path[(i) + (j) * 64]
#define CLR_ALL(flag) (*flag##_cover = 0)
#define SET_FLG(flag, index) (*flag##_cover |= (1 << (index)))
#define CLR_FLG(flag, index) (*flag##_cover &= ~(1 << (index)))
#define TST_FLG(flag, index) (*flag##_cover & (1 << (index)))
/* Global Variable Definition ------------------------------------------------------------------- */
static float g_cost[64];
static int g_mark[64];
static uint8_t g_row_cover;
static uint8_t g_col_cover;
/* Function Declaration ------------------------------------------------------------------------- */
static int step_one(float *cost, int m, int n);
static int step_two(float *cost, int *mark, int m, int n, uint8_t *col_cover);
static int step_three(int *mark, int m, int n, uint8_t *col_cover);
static int step_four(float *cost, int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int *path_start_i, int *path_start_j);
static int step_five(int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int path_start_i, int path_start_j);
static int step_six(float *cost, int m, int n, uint8_t *row_cover, uint8_t *col_cover);
/* Function Definition -------------------------------------------------------------------------- */
int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) {
int i = 0;
int j = 0;
int step;
if (m > 8 || n > 8) {
return MUNKRES_EXCESSIVE_MATRIX;
}
memcpy(g_cost, cost, m * n * sizeof(*g_cost));
memset(g_mark, 0, m * n * sizeof(*g_mark));
g_row_cover = 0;
g_col_cover = 0;
step = 0;
#if PRINT_ALGO_RUN_TIME
time_algo_1 = uTimetick_Get();
#endif
while (7 != step) {
switch (step) {
case 0:
case 1:
step = step_one(g_cost, m, n);
break;
case 2:
step = step_two(g_cost, g_mark, m, n, &g_col_cover);
break;
case 3:
step = step_three(g_mark, m, n, &g_col_cover);
break;
case 4:
step = step_four(g_cost, g_mark, m, n, &g_row_cover, &g_col_cover, &i, &j);
break;
case 5:
step = step_five(g_mark, m, n, &g_row_cover, &g_col_cover, i, j);
break;
case 6:
step = step_six(g_cost, m, n, &g_row_cover, &g_col_cover);
break;
default:
step = 7;
break;
}
}
for (i = 0; i < m; ++i) {
for (j = 0; j < n; ++j) {
if (0 != g_mark[(i) + (j) * m]) {
assignment[i] = j;
break;
}
}
}
return MUNKRES_NO_ERROR;
}
static int step_one(float *cost, int m, int n) {
int i;
int j;
float min;
for (i = 0; i < m; ++i) {
min = COST(i, 0);
for (j = 1; j < n; ++j) {
if (min > COST(i, j)) {
min = COST(i, j);
}
}
for (j = 0; j < n; ++j) {
COST(i, j) -= min;
}
}
return 2;
}
static int step_two(float *cost, int *mark, int m, int n, uint8_t *col_cover) {
int i;
int j;
for (i = 0; i < m; ++i) {
for (j = 0; j < n; ++j) {
if (0 == COST(i, j) && 0 == TST_FLG(col, j)) {
MARK(i, j) = 1;
SET_FLG(col, j);
break;
}
}
}
CLR_ALL(col);
return 3;
}
static int step_three(int *mark, int m, int n, uint8_t *col_cover) {
int i;
int j;
int count;
CLR_ALL(col);
for (i = 0; i < m; ++i) {
for (j = 0; j < n; ++j) {
if (1 == MARK(i, j)) {
SET_FLG(col, j);
}
}
}
count = 0;
for (j = 0; j < n; ++j) {
count += (0 != TST_FLG(col, j) ? 1 : 0);
}
return (count >= m || count >= n) ? 7 : 4;
}
static void find_a_noncovered_zero(int *i, int *j, float *cost, int m, int n, uint8_t *row_cover, uint8_t *col_cover) {
for (*i = 0; *i < m; ++*i) {
if (0 != TST_FLG(row, *i)) {
continue;
}
for (*j = 0; *j < n; ++*j) {
if (0 == COST(*i, *j) && 0 == TST_FLG(col, *j)) {
break;
}
}
if (n != *j) {
break;
}
}
}
static void find_a_star_zero_in_row(int *j, int i, int *mark, int m, int n) {
for (*j = 0; *j < n; ++*j) {
if (1 == MARK(i, *j)) {
break;
}
}
}
static int step_four(float *cost, int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int *path_start_i, int *path_start_j) {
int i;
int j;
int jj;
while (1) {
find_a_noncovered_zero(&i, &j, cost, m, n, row_cover, col_cover);
if (m == i) {
return 6;
}
else {
MARK(i, j) = 2;
find_a_star_zero_in_row(&jj, i, mark, m, n);
if (m != jj) {
SET_FLG(row, i);
CLR_FLG(col, jj);
j = jj;
}
else {
*path_start_i = i;
*path_start_j = j;
return 5;
}
}
}
}
static void find_a_star_zero_in_col(int *i, int j, int *mark, int m) {
for (*i = 0; *i < m; ++*i) {
if (1 == MARK(*i, j)) {
break;
}
}
}
static void find_a_prime_zero_in_row(int *j, int i, int *mark, int m, int n) {
for (*j = 0; *j < n; ++*j) {
if (2 == MARK(i, *j)) {
break;
}
}
}
static int step_five(int *mark, int m, int n, uint8_t *row_cover, uint8_t *col_cover, int path_start_i, int path_start_j) {
static int path[64 * 2];
int i;
int j;
int k;
k = 0;
PATH(k, 0) = path_start_i;
PATH(k, 1) = path_start_j;
while (1) {
find_a_star_zero_in_col(&i, PATH(k, 1), mark, m);
if (m == i) {
goto STEP_FIVE_EXIT;
}
else {
PATH(k + 1, 0) = i;
PATH(k + 1, 1) = PATH(k, 1);
++k;
find_a_prime_zero_in_row(&j, PATH(k, 0), mark, m, n);
PATH(k + 1, 0) = PATH(k, 0);
PATH(k + 1, 1) = j;
++k;
}
}
STEP_FIVE_EXIT:
/* augment path */
for(; k >= 0; --k) {
MARK(PATH(k, 0), PATH(k, 1)) = (1 == MARK(PATH(k, 0), PATH(k, 1)) ? 0 : 1);
}
/* clear covers */
CLR_ALL(row);
CLR_ALL(col);
/* erease primes */
for (i = 0; i < m; ++i) {
for (j = 0; j < n; ++j) {
if (2 == MARK(i, j)) {
MARK(i, j) = 0;
break;
}
}
}
return 3;
}
static int step_six(float *cost, int m, int n, uint8_t *row_cover, uint8_t *col_cover) {
float min;
int i;
int j;
min = 1e6;
for (i = 0; i < m; ++i) {
for (j = 0; j < n; ++j) {
if (0 == TST_FLG(row, i) && 0 == TST_FLG(col, j)) {
if (min > COST(i, j)) {
min = COST(i, j);
}
}
}
}
for (i = 0; i < m; ++i) {
for (j = 0; j < n; ++j) {
if (0 != TST_FLG(row, i)) {
COST(i, j) += min;
}
if (0 == TST_FLG(col, j)) {
COST(i, j) -= min;
}
}
}
return 4;
}

265
1.Software/PDR 1.06/src/PDRBase.c Normal file
View File

@ -0,0 +1,265 @@
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : PDRBase.c
* Current Version : V2.0
* Author : logzhan
* Date of Issued : 2022.10.15
* Comments : PDR基础功能函数
********************************************************************************/
#include <math.h>
#include "PDRBase.h"
#include "Location.h"
#include "LinearFit.h"
#include "Utils.h"
#include "CoordTrans.h"
#include "Kalman.h"
extern double GpsPos[HIST_GPS_NUM][3];
int PredictStep(StepPredict* stepPredict, double timestamp, unsigned long steps_last, float gnssVel)
{
int step = 0;
float mean_time = 400;
double dis = 0;
if (stepPredict->meanTime > 0)
mean_time = stepPredict->meanTime;
if ((steps_last > 0) && (stepPredict->fnum > 0) && (timestamp - stepPredict->lastTime > mean_time * 3) && (stepPredict->lastTime > 0))
{
stepPredict->fsum = stepPredict->fsum / stepPredict->fnum;
dis = sqrt(pow(GpsPos[HIST_GPS_NUM - 1][0] - GpsPos[HIST_GPS_NUM - 2][0], 2) + pow(GpsPos[HIST_GPS_NUM - 1][1] - GpsPos[HIST_GPS_NUM - 2][1], 2)
+ pow(GpsPos[HIST_GPS_NUM - 1][2] - GpsPos[HIST_GPS_NUM - 2][2], 2));
if (stepPredict->fsum != 0.0 && (gnssVel > 1.0 || (dis > 1.0 && dis < 3.0)))
{
step = (int)((timestamp - stepPredict->lastTime) / mean_time);
stepPredict->deltaStep += step;
}
stepPredict->lastTime = timestamp;
stepPredict->fnum = 0;
stepPredict->fsum = 0;
}
return step;
}
double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss)
{
int i;
double lla[3] = { 0.0 };
double ned1[3] = { 0.0 };
double temp[2][HIST_GPS_NUM] = { { 0.0 } };
double angle[HIST_GPS_NUM - 1] = { 0.0 };
double ned2[3] = { 0.0 };
double maxn = 0;
double maxe = 0;
double minn = 0;
double mine = 0;
double meanx = 0;
double meany = 0;
float error = 0;
float sst = 0;
float r2 = 0;
double range = 0;
double yaw = 0;
double tempYaw = 0;
double diffAngle = 0;
double para[3] = { 0 };
int flg = 0;
lla[0] = pgnss.lat;
lla[1] = pgnss.lon;
for (i = 0; i < HIST_GPS_NUM - 1; i++)
{
gpsPos[i][0] = gpsPos[i + 1][0];
gpsPos[i][1] = gpsPos[i + 1][1];
gpsPos[i][2] = gpsPos[i + 1][2];
}
// 经纬度WGS84转地心地固坐标系ECEF
if ((lla[0] > 0) && (lla[1] > 0)){
WGS842ECEF(lla, gpsPos[HIST_GPS_NUM - 1]);
}else{
gpsPos[HIST_GPS_NUM - 1][0] = 0;
gpsPos[HIST_GPS_NUM - 1][1] = 0;
gpsPos[HIST_GPS_NUM - 1][2] = 0;
return -1;
}
if (pgnss.error > 40){
return -1;
}
for (i = 0; i < HIST_GPS_NUM - 1; i++)
{
if ((fabs(gpsPos[i][0]) < 1e-6) && (fabs(gpsPos[i][1]) < 1e-6) && (fabs(gpsPos[i][2]) < 1e-6))
{
return -1;
}
}
//地心地固坐标系ECEF转东北天坐标系NED
ECEF2NED(gpsPos[HIST_GPS_NUM - 1], lla, ned1);
for (i = HIST_GPS_NUM - 2; i >= 0; i--) {
ECEF2NED(gpsPos[i], lla, ned2);
temp[0][i] = ned1[0] - ned2[0];
temp[1][i] = ned1[1] - ned2[1];
meanx += temp[0][i] / HIST_GPS_NUM;
meany += temp[1][i] / HIST_GPS_NUM;
if (maxn > temp[0][i])
{
maxn = temp[0][i];
}
if (minn < temp[0][i])
{
minn = temp[0][i];
}
if (maxe > temp[1][i])
{
maxe = temp[1][i];
}
if (mine < temp[1][i])
{
mine = temp[1][i];
}
angle[i] = atan2(temp[1][i], temp[0][i]);
if (angle[i] < 0)
angle[i] += TWO_PI;
}
flg = leastDistanceLinearFit(temp[0], temp[1], HIST_GPS_NUM, para);
if (flg < 0)
return -1;
for (i = HIST_GPS_NUM - 1; i >= 0; i--) {
error += (float)(pow(para[0] * temp[0][i] + para[1] * temp[1][i] + para[2], 2) / (pow(para[0], 2) + pow(para[1], 2)));
sst += (float)(pow(meanx - temp[0][i], 2) + pow(meany - temp[1][i], 2));
}
if (sst > 0)
{
r2 = 1 - error / sst;
}
range = sqrt(pow(maxn - minn, 2) + pow(maxe - mine, 2));
//printf("%f,%f,%f\n", r2, error, range);
if (r2 < 0.95 || range < 1.5 || error > 3.0)
return -1;
yaw = atan(-para[0] / para[1]);
modAngle(&yaw, 0, TWO_PI);
tempYaw = meanAngle(angle, HIST_GPS_NUM - 1);
modAngle(&tempYaw, 0, TWO_PI);
diffAngle = tempYaw - yaw;
modAngle(&diffAngle, -PI, PI);
if (fabs(diffAngle) > PI / 2)
yaw += PI;
modAngle(&yaw, 0, TWO_PI);
return yaw;
}
/**----------------------------------------------------------------------
* Function : StateRecognition
* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
void StateRecognition(float* acc, Classifer_t* sys) {
}
void calStepLastTime(StepPredict* stepPredict, double timestamp, unsigned long steps_last,
unsigned long steps)
{
float tmpTime = 0;
if (stepPredict->lastTime > 0 && stepPredict->deltaStep == 0 && (steps - steps_last) > 0)
{
tmpTime = (float)((timestamp - stepPredict->lastTime) / (steps - steps_last));
if (tmpTime > 300 && tmpTime < 800)
{
if (stepPredict->meanTime > 0) {
stepPredict->meanTime = stepPredict->meanTime * 0.5f + tmpTime * 0.5f;
}
else {
stepPredict->meanTime = tmpTime;
}
}
}
stepPredict->fnum = 0;
stepPredict->fsum = 0;
stepPredict->lastTime = timestamp;
stepPredict->deltaStep = 0;
}
/**----------------------------------------------------------------------
* 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) {
int step;
long deltaStep;
double stepLength = 0.7;
stepPredict->fnum++;
stepPredict->fsum += pdr->motionFreq;
ulong steps = pdr->steps;
ulong lastSteps = pdr->lastSteps;
if (steps - lastSteps > 0 && pdr->motionFreq != 0.0) {
deltaStep = steps - lastSteps - stepPredict->deltaStep;
// 应该是防止计步器异常情况，一次计步特别多
if (deltaStep > 5)deltaStep = 5;
if (stepPredict->meanTime > 0)
{
step = (int)((imu->gyr.time - stepPredict->lastTime) / stepPredict->meanTime + 0.5);
if (step > deltaStep && step <= 3)deltaStep = step;
}
if (lastSteps == 0)deltaStep = 0;
if (pdr->heading > 0) {
EKFStatePredict(kf, stepLength, pdr, deltaStep);
pdr->fusionPdrFlg = PDR_TRUE;
}
calStepLastTime(stepPredict, imu->gyr.time, lastSteps, steps);
}
else {
// 有时候，计步器可能考虑到稳定性暂时不更新步数，因此需要利用GNSS速度预测步数，达到
// 惯导更新位置的目的
int PreStep = PredictStep(stepPredict, imu->gyr.time, lastSteps, pgnss->vel);
if (pdr->heading > 0) {
EKFStatePredict(kf, stepLength, pdr, PreStep);
if (PreStep > 0)pdr->fusionPdrFlg = PDR_TRUE;
}
}
// 如果计步器可以保持较高的实时性，就可以采用下面的方法直接计算
//if (pdr->heading > 0) {
// deltaStep = steps - lastSteps;
// EKFStatePredict(kf, stepLength, pdr, deltaStep);
// pdr->fusionPdrFlg = PDR_TRUE;
//}
}

891
1.Software/PDR 1.06/src/ParseData.c Normal file
View File

@ -0,0 +1,891 @@
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : pdr_parseData.c
* Department : Sensor Algorithm Team
* Current Version : V1.0
* Author :
* Date of Issued : 2021.01.29
* Comments : GNSS数据解析，解析来自上层传入算法的字符串数据。主要是两种类
型。IMU字符串和NMEA字符串。
IMU格式 : time_ms, type, data_x, data_y, data_z
GPS格式 ：time_ms, type, utc_time(second unit), $GPXXX,...
其中time_ms是时间戳单位，时间单位要求是ms，不是ms需要转换为ms
type是传感器类型码，代表传入字符串所代表的传感器类型。
NMEA协议一般是$G开头。NMEA协议一般会附带utc时间，单位是秒(s)
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <stdio.h>
#include <math.h>
#include <malloc.h>
#include <string.h>
#include <errno.h>
#include <stdlib.h>
#include <stdbool.h>
#include "parseData.h"
#include "pdr_sensor.h"
#include "pdr_api.h"
extern Sensor_t SensorDataHist[IMU_LAST_COUNT];
char * strtok_ct(char * s, const char * delim)
{
static char* lasts;
if (s == 0)s = lasts;
if (*s == '\0')return 0;
lasts = s + strcspn(s, delim);
if (*lasts != 0)*lasts++ = 0;
return s;
}
/**----------------------------------------------------------------------
* Function : Int2Hex
* Description : 十六进制数转换为十进制数
* Date : 2020/7/9 yuanlin@vivo.com & logzhan
*---------------------------------------------------------------------**/
char* Int2Hex(int a, char *buffer)
{
sprintf(buffer, "%X", a);
return (buffer);
}
/**----------------------------------------------------------------------
* Function : HexToDec
* Description : 十六进制数转换为十进制数
* Date : 2022-09-15 logzhan
*---------------------------------------------------------------------**/
long HexToDec(char *source)
{
long sum = 0, t = 1;
int len = (int)strlen(source);
for (int i = len - 1; i >= 0; i--) {
sum += t * pdr_getIndexOfSigns(*(source + i));
t *= 16;
}
return sum;
}
/**----------------------------------------------------------------------
* Function : pdr_getIndexOfSigns
* Description : 返回ch字符在sign数组中的序号
* Date : 2020/7/9 yuanlin@vivo.com &logzhan
*---------------------------------------------------------------------**/
int pdr_getIndexOfSigns(char ch)
{
if (ch >= '0' && ch <= '9')return ch - '0';
if (ch >= 'A' && ch <= 'F')return ch - 'A' + 10;
if (ch >= 'a' && ch <= 'f')return ch - 'a' + 10;
return -1;
}
void ParseIMU(char* pt, IMU_t *imu_p, double ts, int sstp)
{
int i = 0;
int j = 0;
Sensor_t data_xyz = { 0 };
Sensor_t outputData = { 0 };
data_xyz.update = 1;
while (pt && 3 > i)
{
if (0 == i)
{
data_xyz.s[0] = (float)atof(pt);
}
else if (1 == i)
{
data_xyz.s[1] = (float)atof(pt);
}
else if (2 == i)
{
data_xyz.s[2] = (float)atof(pt);
}
data_xyz.time = ts;
pt = strtok_ct(NULL, ",");
i++;
}
#if 0
if (SENSOR_ACC == sstp)
{
memcpy(&imu_p->acc, &data_xyz, sizeof(sensor_agm));
}
else if (SENSOR_MAG == sstp)
{
memcpy(&imu_p->mag, &data_xyz, sizeof(sensor_agm));
}
else if (SENSOR_GYRO == sstp)
{
memcpy(&imu_p->gyro, &data_xyz, sizeof(sensor_agm));
}
#else
data_xyz.type = sstp;
//Hist_SensorData第0位为时间上最靠前的数据
if (SensorDataHist[0].time > data_xyz.time)
{
memcpy(&outputData, &data_xyz, sizeof(Sensor_t));
}
else
{
memcpy(&outputData, &SensorDataHist[0], sizeof(Sensor_t));
for (i = IMU_LAST_COUNT - 1; i >= 0; i--)
{
if ((SensorDataHist[i].time <= data_xyz.time))
{
for (j = 1; j <= i ; j++)
{
memcpy(&SensorDataHist[j - 1], &SensorDataHist[j], sizeof(Sensor_t));
}
memcpy(&SensorDataHist[i], &data_xyz, sizeof(Sensor_t));
break;
}
}
}
if (SENSOR_ACC == outputData.type)
{
memcpy(&imu_p->acc, &outputData, sizeof(Sensor_t));
}
else if (SENSOR_MAG == outputData.type)
{
memcpy(&imu_p->mag, &outputData, sizeof(Sensor_t));
}
else if (SENSOR_GYRO == outputData.type)
{
memcpy(&imu_p->gyr, &outputData, sizeof(Sensor_t));
}
#endif
}
void parseNMEA(char* pt, Nmea_t *ln, double ts)
{
long i, result;
long s = 0 ;
char buffer[3] = {'0','0','\0'};
char* line_p = pt + strlen(pt) + 1;
//char sumBuf[3]
if (strchr(line_p, '*') == NULL)
{
return;
}
for (result = line_p[1], i = 2; line_p[i] != '*'; i++){
result ^= line_p[i];
}
buffer[0] = line_p[i + 1];
buffer[1] = line_p[i + 2];
s = HexToDec(buffer);
if (result == s) { //
pt = strtok_ct(NULL, ",");
/*printf("pt nmea l %s %u %u\n", pt,i, strcmp(pt, GNRMC_STR));*/
if (strstr(pt, GGA_STR))
{
ParseGGA(pt, ln, ts);
}
else if (strstr(pt, RMC_STR))
{
ParseRMC(pt, ln, ts);
}
else if (strstr(pt, GSV_STR))
{
parseGSV(pt, ln, ts);
}
else if (strstr(pt, GSA_STR))
{
ParseGSA(pt, ln, ts);
}
//minTime：应用程序接收GPS更新的最短时间（即只有超过这个时间设定，系统才可能通知我们的程序说“GPS"——单位毫秒
if ((ln->minTime < 1000) || (ln->minTime > ts))
{
ln->minTime = ts;
}
if ((ln->maxTime < 1000) || (ln->maxTime < ts))
{
ln->maxTime = ts;
}
}//
else {
printf("unpass %s", line_p);
}
}
void preprocessNMEA(Nmea_t *ln)
{
if (ln->gga.update && (ln->rmc[0].update || ln->rmc[1].update) && (ln->gsa[0].update || ln->gsa[1].update || ln->gsa[2].update)
&& (ln->gsv[0].update || ln->gsv[1].update || ln->gsv[2].update))
{
ln->update = 1;
}
else
{
NmeaFlag_Init(ln);
}
}
void ParseGGA(char* pt, Nmea_t *ln, double ts)
{
if (!strcmp(pt, GNGGA_STR))
{
ln->gga.type = SENSOR_LOCATION_NMEA_GNGGA;
ln->gga.update = 1;
}
else if (!strcmp(pt, GPGGA_STR))
{
ln->gga.type = SENSOR_LOCATION_NMEA_GPGGA;
ln->gga.update = 1;
}
else
{
return;
}
int i = 0;
pt = strtok_ct(NULL, ",");
while (pt && i < 13)
{
if (pt && '\0' == *pt)
{
if (0 == i)
{
ln->gga.gga_utc = ITEM_INVALID;
}
else if (1 == i)
{
ln->gga.latitude = ITEM_INVALID;
}
else if (2 == i)
{
ln->gga.gga_latitude_ns = LATITUDE_N;
}
else if (3 == i)
{
ln->gga.longitudinal = ITEM_INVALID;
}
else if (4 == i)
{
ln->gga.longitudinal_ew = LATITUDE_N;
}
else if (5 == i)
{
ln->gga.status = invalid;
}
else if (6 == i)
{
ln->gga.satellites_nb = ITEM_INVALID;
}
else if (7 == i)
{
ln->gga.hdop = ITEM_INVALID;
}
else if (8 == i)
{
ln->gga.altitude = ITEM_INVALID;
}
else if (9 == i)
{
ln->gga.unit_a = ITEM_INVALID;
}
else if (10 == i)
{
ln->gga.height = ITEM_INVALID;
}
else if (11 == i)
{
ln->gga.unit_h = ITEM_INVALID;
}
else if (12 == i)
{
ln->gga.dgps = ITEM_INVALID;
}
else if (13 == i)
{
ln->gga.gga_check_sum[0] = '\0';
}
}
else
{
if (0 == i)
{
ln->gga.gga_utc = atof(pt);
}
else if (1 == i)
{
ln->gga.latitude = atof(pt);
}
else if (2 == i)
{
if (!strcmp(pt, NORTH))
{
ln->gga.gga_latitude_ns = LATITUDE_N;
}
else if (!strcmp(pt, SOUTH))
{
ln->gga.gga_latitude_ns = LATITUDE_S;
}
}
else if (3 == i)
{
ln->gga.longitudinal = atof(pt);
}
else if (4 == i)
{
if (!strcmp(pt, EAST))
{
ln->gga.longitudinal_ew = LONGITUDINAL_E;
}
else if (!strcmp(pt, WEST))
{
ln->gga.longitudinal_ew = LONGITUDINAL_W;
}
}
else if (5 == i)
{
ln->gga.status = (gngga_quality)atoll(pt);
}
else if (6 == i)
{
ln->gga.satellites_nb = atoi(pt);
}
else if (7 == i)
{
ln->gga.hdop = (float)atof(pt);
}
else if (8 == i)
{
ln->gga.altitude = (float)atof(pt);
}
else if (9 == i)
{
if (!strcmp(pt, "M"))
{
ln->gga.unit_a = 1;
}
}
else if (10 == i)
{
ln->gga.height = (float)atof(pt);
}
else if (11 == i)
{
if (!strcmp(pt, "M"))
{
ln->gga.unit_h = 1;
}
}
}
pt = strtok_ct(NULL, ",");
i++;
/*printf("pt %s %u\n", pt,i);*/
}
ln->gga.time = ts;
}
void ParseRMC(char* pt, Nmea_t *ln, double ts)
{
int i = 0;
double temp = 0;
double rmc_time = 0;
NmeaRMC_t *rmc = NULL;
if (!strcmp(pt, GNRMC_STR))
{
rmc = &ln->rmc[0];
RMC_Init(rmc);
rmc->type = SENSOR_LOCATION_NMEA_GNRMC;
rmc->update = 1;
}
else if (!strcmp(pt, GPRMC_STR))
{
rmc = &ln->rmc[1];
RMC_Init(rmc);
rmc->type = SENSOR_LOCATION_NMEA_GPRMC;
rmc->update = 1;
}
else
{
return;
}
pt = strtok_ct(NULL, ",");
while (pt && 12>i)
{
if (strlen(pt) > 0)
{
if (0 == i)
{
rmc_time = atof(pt);
rmc->rmc_utc = rmc_time;
}
else if (1 == i)
{
if (!strcmp(pt, "V"))
{
rmc->status = POSITIONING_N;
}
else if (!strcmp(pt, "A"))
{
rmc->status = POSITIONING_Y;
}
}
else if (2 == i)
{
temp = atof(pt);
rmc->latitude = (floor(temp / 100) + (temp - floor(temp / 100) * 100) / 60);
}
else if (3 == i)
{
if (!strcmp(pt, NORTH))
{
rmc->rmc_latitude_ns = LATITUDE_N;
}
else if (!strcmp(pt, SOUTH))
{
rmc->rmc_latitude_ns = LATITUDE_S;
}
}
else if (4 == i)
{
temp = atof(pt);
rmc->longitudinal = (floor(temp / 100) + (temp - floor(temp / 100) * 100) / 60);
}
else if (5 == i)
{
if (!strcmp(pt, EAST))
{
rmc->longitudinal_ew = LONGITUDINAL_E;
}
else if (!strcmp(pt, WEST))
{
rmc->longitudinal_ew = LONGITUDINAL_W;
}
}
else if (6 == i)
{
rmc->speed = (float)atof(pt);
}
else if (7 == i)
{
rmc->cog = (float)atof(pt);
}
else if (8 == i)
{
rmc->utc_ddmmyy = (float)atof(pt);
}
else if (9 == i)
{
rmc->magnetic = (float)atof(pt);
}
else if (10 == i)
{
if (!strcmp(pt, EAST))
{
rmc->mag_h = MAG_E;
}
else if (!strcmp(pt, WEST))
{
rmc->mag_h = MAG_W;
}
}
else if (11 == i)
{
if (!strcmp(pt, "A"))
{
rmc->mode = MODE_A;
}
else if (!strcmp(pt, "D"))
{
rmc->mode = MODE_D;
}
else if (!strcmp(pt, "E"))
{
rmc->mode = MODE_E;
}
else if (!strcmp(pt, "N"))
{
rmc->mode = MODE_N;
}
}
else if (12 == i)
{
}
}
pt = strtok_ct(NULL, ",");
i++;
}
rmc->time = ts;
}
void parseGSV(char* pt, Nmea_t *ln, double ts)
{
char *pt_gsv = NULL;
//memset(&ln->gsv, ITEM_INVALID, sizeof(ln->gsv));
int i = 0;
NmeaGSV_t* gsv = NULL;
if (!strcmp(pt, GPGSV_STR))
{
gsv = &ln->gsv[0];
gsv->type = SENSOR_LOCATION_NMEA_GPGSV;
}
else if (!strcmp(pt, GLGSV_STR))
{
gsv = &ln->gsv[1];
gsv->type = SENSOR_LOCATION_NMEA_GLGSV;
}
else if (!strcmp(pt, GAGSV_STR))
{
gsv = &ln->gsv[2];
gsv->type = SENSOR_LOCATION_NMEA_GAGSV;
}
else
{
return;
}
/*printf("gsv %s %d\n", pt, gsv->type);*/
/*printf("nmea rmc %s %u %u\n", pt, !strcmp(pt, GNRMC_STR), rmc->type);*/
pt = strtok_ct(NULL, ",");
while (pt && 3>i)
{
if (0 == i)
{
gsv->sentences = atoi(pt); //total
}
else if (1 == i)
{
gsv->sentence_number = atoi(pt); // Which term of
if (1 == gsv->sentence_number)
{
gsv->satellite_number = 0;
}
}
else if (2 == i)
{
gsv->satellites = atoi(pt); //total
break;
}
pt = strtok_ct(NULL, ",");
i++;
}
pt_gsv = strtok_ct(NULL, "*");
pt = strtok_ct(pt_gsv, ",");
i = 0;
while (pt && 16>i && gsv->satellites > 0)
{
if (pt && '\0' == *pt)
{
if (0 == i % 4)
{
gsv->satellites_data[gsv->satellite_number].prn = ITEM_INVALID;
}
else if (1 == i % 4)
{
gsv->satellites_data[gsv->satellite_number].elevation = ITEM_INVALID;
}
else if (2 == i % 4)
{
gsv->satellites_data[gsv->satellite_number].azimuth = ITEM_INVALID;
}
else if (3 == i % 4)
{
gsv->satellites_data[gsv->satellite_number].snr = ITEM_INVALID;
if (gsv->satellite_number < gsv->satellites) //satellite_number是第几个;satellites是总共的个数
{
gsv->satellite_number++;
if (gsv->satellite_number == gsv->satellites)
{
break;
}
}
}
}
else
{
if (0 == i % 4)
{
gsv->satellites_data[gsv->satellite_number].prn = atoi(pt);
if (gsv->type == SENSOR_LOCATION_NMEA_GPGSV && gsv->satellites_data[gsv->satellite_number].prn > 32)
{
gsv->satellites_data[gsv->satellite_number].prn = (gsv->satellites_data[gsv->satellite_number].prn % 32) + 1;
}
if (gsv->type == SENSOR_LOCATION_NMEA_GLGSV && gsv->satellites_data[gsv->satellite_number].prn < 65 && gsv->satellites_data[gsv->satellite_number].prn > 99)
{
gsv->satellites_data[gsv->satellite_number].prn = (gsv->satellites_data[gsv->satellite_number].prn % 35) + 65;
}
if (gsv->type == SENSOR_LOCATION_NMEA_GAGSV && gsv->satellites_data[gsv->satellite_number].prn > 36)
{
gsv->satellites_data[gsv->satellite_number].prn = (gsv->satellites_data[gsv->satellite_number].prn % 36) + 1;
}
}
else if (1 == i % 4)
{
gsv->satellites_data[gsv->satellite_number].elevation = atoi(pt); //仰角
}
else if (2 == i % 4)
{
gsv->satellites_data[gsv->satellite_number].azimuth = atoi(pt);
}
else if (3 == i % 4)
{
gsv->satellites_data[gsv->satellite_number].snr = atoi(pt);
// printf("2.gsv->satellites_data[gsv->satellite_number]: %d %d %d %d\n", gsv->satellites_data[gsv->satellite_number].prn, gsv->satellites_data[gsv->satellite_number].elevation, gsv->satellites_data[gsv->satellite_number].azimuth, gsv->satellites_data[gsv->satellite_number].snr);
if (gsv->satellite_number<gsv->satellites) //satellite_number是第几个;satellites是总共的个数
{
gsv->satellite_number++;
if (gsv->satellite_number == gsv->satellites)
{
break;
}
}
}
//printf("2.gsv->satellites_data[gsv->satellite_number]: %d %d %d %d\n", gsv->satellites_data[gsv->satellite_number].prn, gsv->satellites_data[gsv->satellite_number].elevation, gsv->satellites_data[gsv->satellite_number].azimuth, gsv->satellites_data[gsv->satellite_number].snr);
}
pt = strtok_ct(NULL, ",");
i++;
}
if (3 == i % 4 && !pt)
{
gsv->satellites_data[gsv->satellite_number].snr = ITEM_INVALID;
if (gsv->satellite_number<gsv->satellites)
{
gsv->satellite_number++;
}
}
if (gsv->satellites == gsv->satellite_number)
{
gsv->update = 1;
//printf_nmea(gsv->type,ln,PrintLOG_file_name);
}
gsv->time = ts;
}
void ParseGSA(char* pt, Nmea_t *ln, double ts)
{
int i = 0;
int idx = -1;
NmeaGSA_t gsa = {0};
if (!strcmp(pt, GNGSA_STR))
{
gsa.type = SENSOR_LOCATION_NMEA_GNGSA;
gsa.update = 1;
}
else if (!strcmp(pt, GPGSA_STR))
{
idx = 0;
gsa.type = SENSOR_LOCATION_NMEA_GPGSA;
gsa.update = 1;
}
else if (!strcmp(pt, GLGSA_STR))
{
idx = 1;
gsa.type = SENSOR_LOCATION_NMEA_GLGSA;
gsa.update = 1;
}
else if (!strcmp(pt, GAGSA_STR))
{
idx = 2;
gsa.type = SENSOR_LOCATION_NMEA_GAGSA;
gsa.update = 1;
}
else
{
return;
}
// printf("gsa %s %d\n", pt, ln->gsa.type);
pt = strtok_ct(NULL, ",");
/*printf("nmea gngsa %s %u\n", pt, i);*/
while (pt && 17>=i)
{
/*printf("%u gsa pt %s\n", i, pt);*/
if (strlen(pt) > 0)
{
if (0 == i)
{
if (!strcmp(pt, "A"))
{
gsa.mode = POSITIONING_MODE_A;
}
else if (!strcmp(pt, "M"))
{
gsa.mode = POSITIONING_MODE_M;
}
}
else if (1 == i)
{
gsa.p_type = (lct_type)atoll(pt);
}
else if (( i >= 2 ) && ( i <= 13 ))
{
gsa.prn[i - 2] = atoi(pt);
if (idx == 0 && gsa.prn[i - 2] > 32)
{
gsa.prn[i - 2] = (gsa.prn[i - 2] % 32)+1;
}
if (idx == 1 && gsa.prn[i - 2] < 65 && gsa.prn[i - 2] > 99)
{
gsa.prn[i - 2] = (gsa.prn[i - 2] % 35) + 65;
}
if (idx == 2 && gsa.prn[i - 2] > 36)
{
gsa.prn[i - 2] = (gsa.prn[i - 2] % 36)+1;
}
if (gsa.prn[i - 2] > 64)
{
gsa.prnflg |= (1 << (gsa.prn[i - 2] - 64));
if (idx < 0)
{
idx = 1;
}
}
else
{
gsa.prnflg |= (1 << gsa.prn[i - 2]);
if (idx < 0)
{
if (ln->gsa[0].update && ln->gsa[1].update)
{
idx = 2;
}
else
{
idx = 0;
}
}
}
}else if (i == 14){
gsa.pdop = (float)atof(pt);
}else if (i == 15){
gsa.hdop = (float)atof(pt);
}else if (i == 16){
gsa.vdop = (float)atof(pt);
}else if (17 == i)
{
if (idx < 0 && pt[0] == '1')
{
idx = 0;
gsa.type = SENSOR_LOCATION_NMEA_GPGSA;
}
else if (idx < 0 && pt[0] == '2')
{
idx = 1;
gsa.type = SENSOR_LOCATION_NMEA_GLGSA;
}
else if (idx < 0 && pt[0] == '3')
{
idx = 2;
gsa.type = SENSOR_LOCATION_NMEA_GAGSA;
}
}
}
pt = strtok_ct(NULL, ",");
i++;
/*printf("pt %s %u\n", pt,i);*/
}
gsa.time = ts;
if (idx >=0 && idx <=2)
{
memcpy(&ln->gsa[idx], &gsa, sizeof(NmeaGSA_t));
}
}
/**----------------------------------------------------------------------
* Function : parseLocAccuracy
* Description : 解析GPS的Accuracy精度参数
* Date : 2020/7/9 yuanlin@vivo.com &logzhan
*---------------------------------------------------------------------**/
void parseLocAccuracy(char* pt, Nmea_t *ln, double ts)
{
int i = 0;
while (pt && 8>i){
if ((i == 0) && (!strcmp(pt, "Status"))){
pt = strtok_ct(NULL, ",");
ln->accuracy.status = atoi(pt);
return;
}
if (i == 7){
ln->accuracy.update = 1;
ln->accuracy.time = ts;
if ((pt && '\0' == *pt))
{
ln->accuracy.err = -1;
}
else if (ln->accuracy.status == -1 || ln->accuracy.status == 2)
{
ln->accuracy.err = (float)atof(pt);
}
return;
}
pt = strtok_ct(NULL, ",");
i++;
}
}
/**----------------------------------------------------------------------
* Function : ParseLineStr
* Description : 解析字符串信息，根据传感器数据类型，选择不同的函数解析
* Date : 2020/7/9 yuanlin@vivo.com &logzhan
*---------------------------------------------------------------------**/
int ParseGnssInsData(char* line, IMU_t* imu_p, Nmea_t* ln)
{
char* pt = NULL;
int i = 0, sstp = -1;
double ts = 0;
pt = strtok_ct(line, ",");
while (pt && i < 2){
if (i == 0)ts = atoll(pt) * 0.000001;
else if (i == 1)sstp = atoi(pt);
pt = strtok_ct(NULL, ",");
i++;
}
switch (sstp)
{
case SENSOR_ACC:
case SENSOR_MAG:
case SENSOR_GYRO:
ParseIMU(pt, imu_p, ts, sstp);
break;
case SENSOR_LOCATION_STATELLITE:
break;
case SENSOR_LOCATION_NETWORK:
break;
case SENSOR_LOCATION_NMEA:
parseNMEA(pt, ln, ts);
if ((ln->maxTime - ln->minTime) > NAEA_LAST_TIME)
{
preprocessNMEA(ln);
}
break;
case SENSOR_LOCATION_FUSION:
parseLocAccuracy(pt, ln, ts);
break;
}
if ((ln->minTime > 0) && (ts - ln->minTime) > NAEA_LAST_TIME) {
preprocessNMEA(ln);
}
return 0;
}

1245
1.Software/PDR 1.06/src/Pedometer.c Normal file
View File

File diff suppressed because it is too large Load Diff

44
1.Software/PDR 1.06/src/Quaternion.cpp Normal file
View File

@ -0,0 +1,44 @@
#include "math.h"
#include "Quaternion.h"
const float FLT_THRES = 0.000001f; // 浮点数最小阈值
/**---------------------------------------------------------------------
* Function : QuaternionNorm
* Description : 四元数归一化
* Date : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void QuaternionNorm(float* q0, float* q1, float* q2, float* q3) {
float norm = sqrtf(((*q0) * (*q0) + (*q1) * (*q1) +
(*q2) * (*q2) + (*q3) * (*q3)));
if (norm > FLT_THRES) {
norm = 1 / norm;
(*q0) *= norm; (*q1) *= norm;
(*q2) *= norm; (*q3) *= norm;
}
}
/**---------------------------------------------------------------------
* Function : QuaternConj
* Description : 四元数求共轭四元数
* Date : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void QuaternConj(float _q[], float q[]) {
_q[0] = q[0];
_q[1] = -q[1];
_q[2] = -q[2];
_q[3] = -q[3];
}
/**---------------------------------------------------------------------
* Function : QuaternProd
* Description : 四元数乘法
* Date : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void QuaternProd(float qab[], float a[], float b[]) {
qab[0] = a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3];
qab[1] = a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2];
qab[2] = a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1];
qab[3] = a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0];
}

33
1.Software/PDR 1.06/src/Quaternion.h Normal file
View File

@ -0,0 +1,33 @@
#ifndef _PDR_QUATERNION_H_
#define _PDR_QUATERNION_H_
#ifdef __cplusplus
extern "C" {
#endif
/**---------------------------------------------------------------------
* Function : QuaternionNorm
* Description : 四元数归一化
* Date : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void QuaternionNorm(float* q0, float* q1, float* q2, float* q3);
/**---------------------------------------------------------------------
* Function : QuaternConj
* Description : 四元数求共轭四元数
* Date : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void QuaternConj(float _q[], float q[]);
/**---------------------------------------------------------------------
* Function : QuaternProd
* Description : 四元数乘法
* Date : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void QuaternProd(float ab[], float a[], float b[]);
#ifdef __cplusplus
}
#endif
#endif

492
1.Software/PDR 1.06/src/SceneRecognition.c Normal file
View File

@ -0,0 +1,492 @@
/********************************************************************/
/* FILE NAME : scene_recognition.c */
/* AUTHOR : ZhangJingrui, ID: 11082826 */
/* VERSION : V0.85_Beta (Doing) */
/* DESCRIPTION: The definations of functions in scene_recognition.h */
/********************************************************************/
#include "scene_recognition.h"
#include <stdio.h>
#include <math.h> /* fabs() */
#include <stdlib.h> /* malloc(), free() */
/* @Lai Zhilong added below code*/
#if !defined(LOCATION_PLATFORM_QCOM_MODEM) /* The Macro is in "custom.h" */
#include <malloc.h>
/* @Zhang Jingrui: The <malloc.h> header is depercated (and quite Linux specific, */
/* on which it defines non-standard functions like mallinfo(3)). Notice that the */
/* header <stdlib.h> is defined by C89 (and later) standards, but not <malloc.h>. */
#endif
#include <string.h> /* memset() */
#include <float.h> /* FLT_EPSILON */
/* Macro Defination----------------------------------------------*/
/*===============================================================*/
/* Compatible with the value in header file "sensor.h" */
#define _CONSTELLATION_USED_CAPACITY 12 /* Size of array defined as lct_nmea_gsa::prn. */
#define _CONSTELLATION_VISIBLE_CAPACITY 20 /* Size of array defined as lct_nmea_gsv::satellites_data. */
/* Satellite PRN index in _GnssInfo list. */
/* These index are planed with _GNSS_INDEX_LEN. */
/* GP: index in _GnssInfo's list is 0~31, and the PRN index is 1~32 */
#define _GP_INDEX_L 0
#define _GP_INDEX_R 31
#define _GP_INDEX_OFFSET ( _GP_INDEX_L )
#define _GP_SVID_OFFSET 1
/* GA: index in _GnssInfo's list is 32~67, and the PRN index is 1~36 */
#define _GA_INDEX_L 32
#define _GA_INDEX_R 67
#define _GA_INDEX_OFFSET ( _GA_INDEX_L )
#define _GA_SVID_OFFSET 1
/* GL: index in _GnssInfo's list is 68~102, and the PRN index is 65~99 */
#define _GL_INDEX_L 68
#define _GL_INDEX_R 102
#define _GL_INDEX_OFFSET ( _GL_INDEX_L )
#define _GL_SVID_OFFSET 65
/* Global Variable-----------------------------------------------*/
/*===============================================================*/
/* g_gnss_info_buffer: the newnest value is in the 0th sub-stucture. */
static GnssInfo* g_gnss_info_buffer = NULL;
static const int g_gnss_info_buffer_size = _TIME_BUFFER_SIZE;
/* g_gnss_info_buffer_trail: 0 ~ gnss_info_buffer_size */
static int g_gnss_info_buffer_trail = 0;
static _SCENE_MODEL_WORK_STATE g_model_state = MODEL_OFF;
/* static function Declaration-----------------------------------*/
/*===============================================================*/
/* Return @ _is_occupied: 0 -- non resource occupied. */
static int check_resources_occupied(void);
/* Return @ _is_failed: 0 -- process of free res is successful. */
static int free_resources (void);
/* Return @ _is_failed: 0 -- process of allocating res is successful. */
static int alloc_resources (void);
/* Function Name: static _lctnmea_to_gnssinfo() */
/* Usage: Transform from the updated lct_nmea structure to the _GnssInfo structure. */
/* The defination of lct_nmea is in <sensors.h>. */
/* Param @ _dst : The pointer to the target _GnssInfo structure variable.*/
/* Param @ _src_nmea : The pointer to the source lct_nmea structure varible. */
/* Return @ _parse_res_state: The transformation process result.
0--success;
-1--NULL param inputed;
1--lct_nmea _src_nmea is not valiable beacause of the update flag. */
static int lctnmea2Gnssinfo (GnssInfo *, const Nmea_t *);
/* Function name: static _detect_good_signal() */
/* Usage: judge the quality of positioning result between good and bad. */
/* Param @ _src: The pointer to the _GnssInfo structure in the positioning epoch. */
/* The value in the structure should be avaliable. */
/* Return @ _process_res: the quality of positioning result. */
/* _SIGNAL_QUALITY::GOOD -- the signal is good. */
/* _SIGNAL_QUALITY::BAD -- the signal is not good. */
static SIGNAL_QUALITY detGnssSignal (const GnssInfo *);
/* Function name: static _gnss_info_st_cpy() */
/* Usage: Deep copy from one _GnssInfo to another one. */
/* The rule of copy operation: */
/* _dst | _src | Operation behavior */
/* -------------------------------------- */
/* NULL | variable | Nothing done. */
/* NULL | NULL | Nothing done. */
/* val A | NULL | Set the value of A to the initialization value. */
/* val A | val B | Deep copy B to A. */
/* Val A | val A | Nothing done. */
/* Param @ _src: Point to the source _GnssInfo structure. */
/* Param @ _dst: Point to the target _GnssInfo structure . */
/* Return @ (void) */
static void gnss_info_st_cpy (GnssInfo *_dst, const GnssInfo *_src);
/* external function defination ---------------------------------*/
/*===============================================================*/
SCENE_INIT_STATE SceneRecog_Init(void)
{
SCENE_INIT_STATE res = INIT_NOT_PERFORMED;
int _is_occupied = -1;
int is_failed = 0;
/* Check the legality of initialization process. */
if (g_model_state == MODEL_ON)
{
return INIT_ERROR_REDUNDANCY; /* 重复初始化 */
}
/* Initialization of resources */
g_model_state = MODEL_OFF;
_is_occupied = check_resources_occupied();
if (_is_occupied)
{
free_resources();
}
is_failed = alloc_resources();
if (is_failed)
{
res = INIT_ERROR_MEMORY_INIT_FAILED;
}else{
res = INIT_SUCCESS;
g_model_state = MODEL_ON;
}
return res;
}
SCENE_INIT_STATE scene_recognition_reset(void)
{
SCENE_INIT_STATE _process_res = INIT_RESET_FAILED;
int _is_occupied = 0;
int _is_failed = 0;
if (g_model_state == MODEL_OFF)
{
_process_res = INIT_RESET_FAILED;
}
else
{
_is_occupied = check_resources_occupied();
if (_is_occupied)
{
free_resources();
}
g_model_state = MODEL_OFF;
_is_failed = alloc_resources();
if (_is_failed)
{
_process_res = INIT_RESET_FAILED;
g_model_state = MODEL_OFF;
}
else
{
_process_res = INIT_RESET_SUCCESS;
g_model_state = MODEL_ON;
}
}
return _process_res;
}
/**----------------------------------------------------------------------
* Function : sceneRecognitionProc
* Description : GNSS场景识别处理流程：
* 1）将nmea转换为gnss
* 2) 通过accuracy、snr、卫星数量分析GNSS信号质量
* 3) 返回结果
* Input : PDR的nmea结构体
* Return : 场景识别结果
* Author : logzhan
* Date : 2020/02/18
*---------------------------------------------------------------------**/
SCENE_RECOGNITION_RESULT sceneRecognitionProc(const Nmea_t *nmea)
{
SCENE_RECOGNITION_RESULT res = RECOG_UNKNOWN;
int transRes = 0;
SIGNAL_QUALITY detRes = SIG_UNKNOWN;
GnssInfo gnssInfo;
// 将nmea转换为gnss
transRes = lctnmea2Gnssinfo(&gnssInfo, nmea);
switch (transRes)
{
case -1:
res = RECOG_ERROR_PARAM_INCRECT;
break;
case 1:
res = RECOG_ERROR_NMEA_INFO_MISS;
break;
case 0:
// GNSS信号检测结果
detRes = detGnssSignal(&gnssInfo);
if (detRes == GOOD)
{ res = RECOG_OPEN_AREA; }
else if (detRes == BAD)
{ res = RECOG_MULTIPATH; }
else /*UNKOWN but is not exist*/
{ res = RECOG_UNKNOWN; }
break;
default:
res = RECOG_UNKNOWN;
}
return res;
}
/**----------------------------------------------------------------------
* Function : isOpenArea
* Description : GNSS开阔地检测，通过输入GNSS信号，分析当前信号质量，从而确定
* 目标是否处于开阔场地。
* Return : 0 : 非开阔地 1：开阔地(信号质量较好)
* Author : Zhang Jingrui, Geek ID: 11082826
* Date : 2020/02/18 logzhan
*---------------------------------------------------------------------**/
int isOpenArea(const Nmea_t *nmea)
{
SCENE_RECOGNITION_RESULT type = sceneRecognitionProc(nmea);
// 如果是开阔地则返回1
return (type == RECOG_OPEN_AREA);
}
SCENE_DESTROY_STATE scene_recognition_destroy(void)
{
SCENE_DESTROY_STATE res = DESTROY_INVALID;
if (g_model_state == MODEL_OFF){
res = DESTROY_INVALID;
}else{
// 释放资源
free_resources();
g_model_state = MODEL_OFF;
res = DESTROY_SUCCESS;
}
return res;
}
/* static function defination -----------------------------------*/
/*===============================================================*/
static int check_resources_occupied(void)
{
int res = 0;
if (g_gnss_info_buffer != NULL){
++res;
}
return res;
}
static int free_resources(void)
{
int res = 0;
if (g_gnss_info_buffer != NULL)
{
free(g_gnss_info_buffer);
}
g_gnss_info_buffer = NULL; /* The pointer should be set NULL after free() for memory safety. */
g_gnss_info_buffer_trail = 0; /* 0 because of the memory of g_gnss_info_buffer is NULL now. */
return res;
}
static int alloc_resources(void)
{
int _process_res = 0;
if (g_gnss_info_buffer != NULL)
{
/* Check the buffer's occupied. This branch is generally because of the */
/* functions flow call errors. */
_process_res = -1;
return _process_res;
}
GnssInfo *ptr = (GnssInfo *)malloc(sizeof(GnssInfo) * g_gnss_info_buffer_size);
if (ptr != NULL)
{
g_gnss_info_buffer = ptr;
g_gnss_info_buffer_trail = 0;
_process_res = 0;
}
else
{
/* Failed to apply for memory.*/
_process_res = 1;
}
return _process_res;
}
static int lctnmea2Gnssinfo(GnssInfo *_dst, const Nmea_t * _src_nmea)
{
int _parse_res_state = 0;
/* Temporarily deprecated. */
/* int _gsa_used_prn[3][_CONSTELLATION_USED_CAPACITY] = {0}; */ /* GPGSA, GLGSA, GAGSA */
int _i_first = 0;
int _i_second = 0;
int _offset[6] = { _GP_SVID_OFFSET, _GP_INDEX_OFFSET,
_GL_SVID_OFFSET, _GL_INDEX_OFFSET,
_GA_SVID_OFFSET, _GA_INDEX_OFFSET };
int _index = -1;
/* Detecting the NULL pamrameters inputed. */
if (_src_nmea == NULL || _dst == NULL)
{
_parse_res_state = -1;
}
/* Detecting the _src_nmea if it wasn't updated. */
else if (_src_nmea->update == 0)
{
_parse_res_state = 1;
}
/*****************************************************************/
/* DELETE: abandon this branch for the stability of the program. */
/* Detecting the incorrect accuracy value. */
/*else if ((_src_nmea->accuracy.update == 0) ||
(_src_nmea->accuracy.update == 0) )
{
_parse_res_state = 2;
}*/
/****************************************************************/
/* update the the _GnssInfo. */
else
{
/* initialize the memory space pointed by _dst to all 0. */
gnss_info_st_cpy(_dst, NULL);
/* pick the raw accuracy from lct_nmea::loc_accuracy. Therefore _dst->accuracy
could be equal with ITEM_INVALID. */
_dst->accuracy = _src_nmea->accuracy.err;
/* pick the aviable satellites PRN from GxGSA and match the SNR value in
GxGSV where Gx is the presented to GP, GA or GL. */
for (_i_first = 0 ; _i_first < 3 ; ++ _i_first) /* 0 - GP, 1 - GL, 2 - GA */
{
/* make sure the GxGSA and GxGSV has been update in the epoch */
if (_src_nmea->gsa[_i_first].update == 0 || _src_nmea->gsv[_i_first].update == 0)
{
continue;
}
/* copy all the PRN number in GSA to the _dst pointed _GnssInfo */
for (_i_second = 0 ;
_i_second < _CONSTELLATION_USED_CAPACITY &&
_src_nmea->gsa[_i_first].prn[_i_second] != 0 &&
_src_nmea->gsa[_i_first].prn[_i_second] != ITEM_INVALID ;
++ _i_second)
{
_index = _src_nmea->gsa[_i_first].prn[_i_second]
- _offset[_i_first*2] + _offset[_i_first*2 + 1];
if (_index > 0)
{
_dst->sat_used_list[_index] = 1;
}
}
/* copy all the SNR values in GSV to the _dst pointed _GnssInfo */
for (_i_second = 0 ;
(_i_second < _CONSTELLATION_VISIBLE_CAPACITY) &&
_src_nmea->gsv[_i_first].satellites_data[_i_second].snr != ITEM_INVALID ;
++ _i_second)
{
_index = _src_nmea->gsv[_i_first].satellites_data[_i_second].prn
- _offset[_i_first*2] + _offset[_i_first*2 + 1];
if (_index > 0)
{
_dst->snr_list[_index] = (float)(_src_nmea->gsv[_i_first].satellites_data[_i_second].snr);
}
}
}
_dst->update = 1;
_parse_res_state = 0;
}
return _parse_res_state;
}
/**---------------------------------------------------------------------
* Function : detGnssSignal
* Description : 检测GPS信号是否良好，原理：
* 1）当Accuracy在0到5之间，那么是较好的信号
* 2）如果Accuracy在5到10之间，那么利用卫星总数和使用数量判断
* GPS质量好坏
* 3）如果Accuracy在10以外，那么说明GPS信号不好
* Date : 2020/02/18 logzhan
*---------------------------------------------------------------------**/
static SIGNAL_QUALITY detGnssSignal(const GnssInfo* _src)
{
SIGNAL_QUALITY procRes = SIG_UNKNOWN;
int i = 0;
float accuracy = 0;
float snr_large_rate = 0;
int sat_used_num = 0; // 卫星使用数量
/* Simulating a Queue data structure which can be optimazated in the future. */
/* move all the elements to the right one memory and abandon the rightmost one.*/
for (i = 0 ; i < g_gnss_info_buffer_size - 1 ; ++ i)
{
gnss_info_st_cpy((g_gnss_info_buffer + i + 1) , (g_gnss_info_buffer + i) );
}
/* Enqueue where the new element struct is put in the leftmost gpsFusionLocation of the array. */
gnss_info_st_cpy((g_gnss_info_buffer), _src);
if (g_gnss_info_buffer_trail < g_gnss_info_buffer_size)
{
++ g_gnss_info_buffer_trail;
}
/* make sure _src->accuracy is available otherwise the quality of signal is not correct. */
if (fabs(_src->accuracy - ITEM_INVALID) < FLT_EPSILON )
{
procRes = SIG_UNKNOWN;
return procRes;
}
/* features extraction */
for (i = 0 ; i < _GNSS_INDEX_LEN ; ++ i)
{
if (_src->sat_used_list[i] == 1 && _src->snr_list[i] > 20)
{
++ snr_large_rate;
}
}
sat_used_num = _src->sat_visible_number;
accuracy = _src->accuracy;
snr_large_rate = sat_used_num > 0 ? (snr_large_rate / sat_used_num) : 0 ;
/* classification process BEGIN */
if (accuracy > 0 && accuracy < (5 + FLT_EPSILON) )
{ procRes = GOOD; }
else if (accuracy > (10 + FLT_EPSILON) )
{ procRes = BAD; }
else
{
/* 5 <= _accuracy <= 10 */
if (snr_large_rate < 0.878676)
{
if (snr_large_rate < 0.781746)
{ procRes = BAD; }
else
{
if (sat_used_num < 18.5)
{ procRes = BAD; }
else
{ procRes = GOOD; }
}
}else{
if (accuracy > 0 && accuracy < 9.2)
{ procRes = GOOD; }
else
{ procRes = BAD; }
}
}
/* classification process END */
return procRes;
}
static void gnss_info_st_cpy(GnssInfo *_dst, const GnssInfo *_src)
{
if (_dst != NULL && _src == NULL)
{
/* initialize _dst if _src is a empty pointer. */
_dst->update = 0;
_dst->local_timestamp = 0;
_dst->accuracy = 0;
_dst->sat_visible_number = 0;
memset(_dst->snr_list, 0, sizeof(_dst->snr_list) );
memset(_dst->sat_used_list, 0, sizeof(_dst->sat_used_list) );
}
else if (_dst != NULL && _src != NULL && _dst != _src)
{
/* deep copy from _src to _dst if both of them isn't empty and not equal. */
_dst->update = _src->update;
_dst->local_timestamp = _src->local_timestamp;
_dst->accuracy = _src->accuracy;
_dst->sat_visible_number = _src->sat_visible_number;
memcpy(_dst->snr_list, _src->snr_list, sizeof(_src->snr_list) );
memcpy(_dst->sat_used_list, _src->sat_used_list, sizeof(_src->sat_used_list) );
}
else
{
/* _dst and _src are both NULL pointer, then PASS. */
/* _dst is NULL pointer, then PASS. */
/* _dst and _src are both pointer to the same memory, then PASS. */
}
}

465
1.Software/PDR 1.06/src/Utils.c Normal file
View File

@ -0,0 +1,465 @@
/******************** (C) COPYRIGHT 2020 Geek************************************
* File Name : pdr_util.c
* Department : Sensor Algorithm Team
* Current Version : V1.2
* Author : logzhan
* Date of Issued : 2020.7.4
* Comments : PDR 工具类函数支持
********************************************************************************/
/* Header File Including -----------------------------------------------------*/
#include <stdio.h>
#include <math.h>
#include <float.h>
#include "PDRBase.h"
#include "Utils.h"
#include "CoordTrans.h"
const float FLT_THRES = 0.000001f; // 浮点数最小阈值
/* Function Declaration ------------------------------------------------------*/
/**----------------------------------------------------------------------
* Function : mean
* Description : 对输入double数组求均值
* Review : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用
* 不同函数
* Date : 2020/7/4 logzhan
*---------------------------------------------------------------------**/
double mean(double *data, int len) {
int i;
double value = 0;
for (i = 0; i < len; i++) {
value += data[i] / len;
}
return value;
}
/**----------------------------------------------------------------------
* Function : fmean
* Description : 对输入float数组求均值
* Review : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用
* 不同函数
* Date : 2020/7/4 logzhan
*---------------------------------------------------------------------**/
float fmean(float *data, int len)
{
float value = 0;
for (int i = 0; i < len; i++){
value += data[i] / len;
}
return value;
}
double meanAngle(double* angle, int len)
{
double baseAngle = angle[0];
double diff = 0.0;
double sum = 0.0;
double value = 0;
if (len <= 0)return 0;
for (int i = 0; i < len; i++)
{
diff = angle[i] - baseAngle;
if (diff > PI)
{
diff = diff - TWO_PI;
}
else if (diff < -PI)
{
diff = diff + TWO_PI;
}
sum += (baseAngle + diff);
}
value = sum / len;
modAngle(&value, 0, TWO_PI);
return value;
}
void modAngle(double * angle, double min, double max)
{
double value = *angle;
double range = max - min;
while (value > max)value = value - range;
while (value < min)value = value + range;
*angle = value;
}
float stdf(float* data, int len)
{
int i;
float meanValue = fmean(data, len);
float value = 0;
for (i = 0; i < len; i++)
{
value += (data[i] - meanValue) * (data[i] - meanValue) / len;
}
return (float)sqrt(value);
}
/**----------------------------------------------------------------------
* Function : pdr_invSqrt
* Description : 计算1/sqrt(x)的快速的算法
* Date : 2020/6/30 logzhan
*---------------------------------------------------------------------**/
float InvSqrt(float x) {
float xHalf = 0.5f * x;
int i = *(int *)&x;
i = 0x5f3759df - (i >> 1);
x = *(float *)&i;
x = x * (1.5f - xHalf * x * x);
return x;
}
/**---------------------------------------------------------------------
* Function : Vec3Norm
* Description : 三维浮点数向量归一化
* Date : 2022/11/1 logzhan
*---------------------------------------------------------------------**/
void Vec3Norm(float* vx, float* vy, float* vz)
{
float norm = sqrtf(((*vx) * (*vx) + (*vy) * (*vy) +
(*vz) * (*vz)));
// 防止出现模为0的情况
if (norm > FLT_THRES) {
norm = 1 / norm;
(*vx) *= norm; (*vy) *= norm; (*vz) *= norm;
}
}
/**----------------------------------------------------------------------
* Function : Motion2TypeStr
* Description : 把用户运动类型转换为字符串输出
* Date : 2022/9/16 logzhan
*---------------------------------------------------------------------**/
const char* Motion2TypeStr(int type)
{
if (type == PDR_MOTION_TYPE_STATIC) {
return "STATIC";
}
else if (type == PDR_MOTION_TYPE_IRREGULAR) {
return "IRREGULAR";
}
else if (type == PDR_MOTION_TYPE_HANDHELD) {
return "HANDLED";
}
else if (type == PDR_MOTION_TYPE_SWINGING) {
return "SWINGING";
}
return "UNKOWN";
}
/**----------------------------------------------------------------------
* Function : qnb2att
* Description : 四元数转欧拉角
* Date : 2020/7/4 logzhan
*---------------------------------------------------------------------**/
void Qnb2Att(float* q, double* attitude)
{
double q11 = q[0] * q[0]; double q13 = q[0] * q[2];
double q12 = q[0] * q[1]; double q14 = q[0] * q[3];
double q22 = q[1] * q[1]; double q24 = q[1] * q[3];
double q23 = q[1] * q[2]; double q33 = q[2] * q[2];
double q34 = q[2] * q[3]; double q44 = q[3] * q[3];
if (fabs(q34 + q12) <= 0.5) {
attitude[0] = asin(2 * (q34 + q12));
}
attitude[1] = atan2(-2 * (q24 - q13), q11 - q22 - q33 + q44);
attitude[2] = atan2(-2 * (q23 - q14), q11 - q22 + q33 - q44);
}
void GetCovMatrix(double *x, double *y, double cov[2][2], int n) {
int i;
double sum_x = 0, sum_y = 0, sum_xy = 0;
double m_x = 0, m_y = 0;
m_x = mean(x, n);
m_y = mean(y, n);
for (i = 0; i < n; i++) {
sum_xy += (x[i] - m_x)*(y[i] - m_y);
sum_x += (x[i] - m_x)*(x[i] - m_x);
sum_y += (y[i] - m_y)*(y[i] - m_y);
}
cov[0][0] = sum_x / n;
cov[0][1] = sum_xy / n;
cov[1][0] = sum_xy / n;
cov[1][1] = sum_y / n;
}
int Jacobi(double a[][2], double p[][2], int n, double eps, int T)
{
int i, j, k;
double max = a[0][1];
int row = 0;
int col = 0;
int ite_num = 0;
double theta = 0;
double aii = 0;
double ajj = 0;
double aij = 0;
double sin_theta = 0;
double cos_theta = 0;
double sin_2theta = 0;
double cos_2theta = 0;
double arowk = 0;
double acolk = 0;
double pki = 0;
double pkj = 0;
for (i = 0; i < n; i++)
p[i][i] = 1;
while (1)
{
max = fabs(a[0][1]);
row = 0;
col = 1;
for (i = 0; i < n; i++)
for (j = 0; j < n; j++)
if (i != j && fabs(a[i][j])>max)
{
max = fabs(a[i][j]);
row = i;
col = j;
}
if (max < eps)
{
//printf("max : %lf \n", max);
//printf("t : %lf \n", ite_num);
return 1;
}
if (ite_num>T)
{
//printf("max : %lf \n", max);
//printf("t : %lf \n", ite_num);
return 0;
}
if (a[row][row] == a[col][col])
theta = PI / 4;
else
theta = 0.5*atan(2 * a[row][col] / (a[row][row] - a[col][col]));
aii = a[row][row];
ajj = a[col][col];
aij = a[row][col];
sin_theta = sin(theta);
cos_theta = cos(theta);
sin_2theta = sin(2 * theta);
cos_2theta = cos(2 * theta);
a[row][row] = aii * cos_theta*cos_theta + ajj * sin_theta*sin_theta + aij * sin_2theta;
a[col][col] = aii * sin_theta*sin_theta + ajj * cos_theta*cos_theta - aij * sin_2theta;
a[row][col] = 0.5*(ajj - aii)*sin_2theta + aij * cos_2theta;
a[col][row] = a[row][col];
for (k = 0; k < n; k++)
{
if (k != row && k != col)
{
arowk = a[row][k];
acolk = a[col][k];
a[row][k] = arowk * cos_theta + acolk * sin_theta;
a[k][row] = a[row][k];
a[col][k] = acolk * cos_theta - arowk * sin_theta;
a[k][col] = a[col][k];
}
}
if (ite_num == 0)
{
p[row][row] = cos_theta;
p[row][col] = -sin_theta;
p[col][row] = sin_theta;
p[col][col] = cos_theta;
}
else
{
for (k = 0; k < n; k++)
{
pki = p[k][row];
pkj = p[k][col];
p[k][row] = pki * cos_theta + pkj * sin_theta;
p[k][col] = pkj * cos_theta - pki * sin_theta;
}
}
ite_num++;
}
}
static int isRealLat(double lat){ return (lat < 90) && (lat > -90); }
static int isRealLon(double lon){ return (lon < 180) && (lon > -180); }
/*
function: 计算地图上两个由经纬度表示点间的直线距离。
param @ pointA : 点A的经纬度
param @ pointB : 点B的经纬度
return @ dis : A、B点间的距离；若小于0则说明A或B点的经纬度输入有误。
*/
double CalDistance(LatLng_t pointA, LatLng_t pointB)
{
double dis = -1;
double lla1[3] = { 0 };
double lla2[3] = { 0 };
double ned1[3] = { 0 };
double ned2[3] = { 0 };
double diff_ned[3] = { 0 };
lla1[0] = pointA.lat * RADIAN_PER_DEG;
lla1[1] = pointA.lon * RADIAN_PER_DEG;
lla2[0] = pointB.lat * RADIAN_PER_DEG;
lla2[1] = pointB.lon * RADIAN_PER_DEG;
if (isRealLat(pointA.lat) && isRealLon(pointA.lon)
&& isRealLat(pointB.lat) && isRealLon(pointB.lon))
{
WGS842NED(lla1, lla1, ned1);
WGS842NED(lla2, lla1, ned2);
diff_ned[0] = ned1[0] - ned2[0];
diff_ned[1] = ned1[1] - ned2[1];
diff_ned[2] = ned1[2] - ned2[2];
dis = sqrt(pow(diff_ned[0], 2) + pow(diff_ned[1], 2) + pow(diff_ned[2], 2));
}
return dis;
}
LatLng_t ProjPointOfLatLng(LatLng_t point, LatLng_t linePointA, LatLng_t linePointB)
{
/* local variables declaration */
double gradxB_P; /* linePointB 向 point 的 x 梯度 */
double gradxB_A; /* linePointA 向 linePointB 的 x 梯度 */
double gradyB_A; /* linePointA 向 linePointB 的 y 梯度 */
double normPow; /* linePointA 向 linePointB 梯度的平方和 */
double scalarMultip; /* gradxB_A * gradyB_A 的标量乘法 */
LatLng_t resProjPoint; /* 结果点坐标 */
/* block process */
gradxB_A = linePointA.lat - linePointB.lat;
gradyB_A = linePointA.lon - linePointB.lon;
if ((fabs(gradxB_A) < DBL_EPSILON) && (fabs(gradyB_A) < DBL_EPSILON))
{
/* 当linePointA 和linePointB是同一点时，返回的point坐标 */
resProjPoint.lat = point.lat;
resProjPoint.lon = point.lon;
}
else
{
gradxB_P = point.lat - linePointB.lat;
normPow = pow(gradxB_A, 2) + pow(gradyB_A, 2);
resProjPoint.lon = (double)(
(gradxB_P * gradxB_A * gradyB_A + point.lon * pow(gradyB_A, 2) + linePointB.lon * pow(gradxB_A, 2)) / normPow);
if ((fabs(gradxB_A) < DBL_EPSILON) || (fabs(gradyB_A) < DBL_EPSILON))
{
/* 当linePointA 和 linePointB 所在直线与 x轴或y轴平行时做规避*/
scalarMultip = (fabs(gradxB_A) < DBL_EPSILON) ? gradyB_A : gradxB_A;
}
else
{
scalarMultip = gradxB_A * gradyB_A;
}
resProjPoint.lat = (double)(
(gradxB_A * linePointB.lat * gradyB_A + gradxB_A * gradxB_A * (resProjPoint.lon - linePointB.lon)) / scalarMultip);
if (fabs(gradxB_A) < DBL_EPSILON)
{
resProjPoint.lat = linePointA.lat;
}
if (fabs(gradyB_A) < DBL_EPSILON)
{
resProjPoint.lon = point.lon;
}
}
return resProjPoint;
}
/**----------------------------------------------------------------------
* Function : CalEarthParameter
* Description : 根据输入纬度返回该纬度地球相关参数
* Date : 2020/7/8 logzhan
*---------------------------------------------------------------------**/
EarthData_t CalEarthParameter(double lat)
{
double temp_value = 0;
EarthData_t earthData = { 0 };
if (fabs(lat) >= PI / 2)lat = 0.0f;
temp_value = sqrt(1 - WGS84_ECCENTR2 * (sin(lat)*sin(lat)));
earthData.lat = lat;
earthData.rmh = WGS84_RE * (1 - WGS84_ECCENTR2) / (temp_value*temp_value*temp_value);
earthData.rnh = WGS84_RE * cos(lat) / temp_value;
return earthData;
}
/**----------------------------------------------------------------------
* Function : pdr_CalRadianDifferent
* Description : 计算弧度之差
* Date : 2020/7/8 logzhan
*---------------------------------------------------------------------**/
double CalRadianDifferent(double s_dir, double d_dir)
{
double dirDiff = d_dir - s_dir;
if (fabs(dirDiff) < PI)return dirDiff;
return dirDiff > 0 ? dirDiff - 2 * PI : dirDiff + 2 * PI;
}
void ProjPointOfLatLng_cir(double point1[], double yaw, double point2[], double result[])
{
if (fabs(fabs(yaw) - PI / 2) < DOUBLE_ZERO)
{
result[0] = point1[0];
result[1] = point2[1];
return;
}
EarthData_t earthData = CalEarthParameter(point1[0]);
double k = -tan(yaw);
double lonDiff = (point2[1] - point1[1]) * earthData.rnh;
double latDiff = (point2[0] - point1[0]) * earthData.rmh;
double x = (k*latDiff + k * k * lonDiff) / (1 + k * k);
double y = (latDiff + k * lonDiff) / (1 + k * k);
result[1] = point1[1] + x / earthData.rnh;
result[0] = point1[0] + y / earthData.rmh;
}
/**----------------------------------------------------------------------
* Function : pdr_WriteCsvStr
* Description : 将字符串信息写入文本，主要是对fprintf封装一层
* Date : 2020/7/8 logzhan
*---------------------------------------------------------------------**/
void WriteCsvStr(FILE* fp_write, char* str)
{
if (NULL == fp_write)return;
fprintf(fp_write, "%s\n", str);
}
int pdr_min(int a, int b){
return (a > b ? b : a);
}
/**----------------------------------------------------------------------
* Function : pdr_utc2hms
* Description : 将UTC时间转换为时分秒
* Date : 2020/7/8 logzhan
*---------------------------------------------------------------------**/
void pdr_utc2hms(double utc, double* h, double* m, double* s) {
long lutc = (long)(utc / 1000);
lutc = lutc % (3600 * 24);
*h = floor((double)(lutc / (3600)));
*m = floor((lutc - (*h) * 3600) / 60);
*s = lutc - (*h) * 3600 - (*m) * 60;
}

7
1.Software/PDR 1.06/src/imu.c Normal file
View File

@ -0,0 +1,7 @@
/* Header File Including ------------------------------------------------------------------------ */
#include <stdint.h>
#include <math.h>
#include <string.h>
#include "imu.h"
#include "ahrs.h"
#include "detector.h"

7
1.Software/PDR 1.06/src/m_gps.c Normal file
View File

@ -0,0 +1,7 @@
/* Header File Including ------------------------------------------------------------------------ */
#include <string.h>
#include <math.h>
#include "m_gps.h"
#include "pdr_util.h"
#include "pdr.h"