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

优化部分函数命名，去除PCA计算方向部分

PDRHistoryBranch
詹力 2022-10-16 23:21:37 +08:00
parent 868421702a
commit 4028c676c4
11 changed files with 347 additions and 851 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
1.Software/PDR 1.04/include/Kalman.h
View File

@ -47,14 +47,6 @@ void EKFCalQRMatrix(Nmea_t* nmea_data, PDR_t* g_pdr, Classifer_t* sys, EKFPara_t
void EKFStateUpdate(EKFPara_t* kf, GNSS_t* pgnss, Classifer_t* sys, PDR_t* g_pdr,
int scene_type);
/**----------------------------------------------------------------------
* Function : pdr_gnssInsLocFusion
* Description : PDR的GNSS和INS融合定位
* Date : 2020/07/09 logzhan
*---------------------------------------------------------------------**/
void GnssInsLocFusion(GNSS_t* pgnss, PDR_t* g_pdr, Classifer_t* sys, double yaw_bias,
EKFPara_t* kf, LctFs_t* res);
#ifdef __cplusplus
}
#endif

44
1.Software/PDR 1.04/include/Location.h
View File

@ -17,7 +17,7 @@ extern "C" {
* Description : PDR导航系统初始化
* Date : 2022/9/16
*---------------------------------------------------------------------**/
void PDRNav_Init(void);
void PDR_Init(void);
/**---------------------------------------------------------------------
* Function : InsLocation
@ -86,10 +86,48 @@ int GnssInsFusionLocation(Nmea_t* nmea_data, EKFPara_t* kf, LctFs_t* result);
* Description : PDS GNSS定位信息初始化
* Date : 2020/2/1 logzhan
*---------------------------------------------------------------------**/
void InitGnssInfo(void);
void InitGnssInfo(void);
void GnssUpdate(GNSS_t* gps, Nmea_t* nmea);
/**----------------------------------------------------------------------
* Function : GnssUpdate
* Description : nmea数据结构转gnss数据
* Date : 2020/07/08 logzhan
* 2020/02/09 logzhan : 函数对应原pdr_gpsUpdate函数，
* 经过引用分析发现每次GPS更新拷贝gnss结构体和nmea结构体其实没
* 什么用，还增加运算量
*---------------------------------------------------------------------**/
void GnssUpdate(GNSS_t* gps, Nmea_t* nmea);
/**----------------------------------------------------------------------
* Function : OutputRawGnssPos
* Description : 输出GPS位置
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void OutputRawGnssPos(GNSS_t* pgnss, LctFs_t* result);
/**----------------------------------------------------------------------
* Function : ResetOutputLoction
* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数
* 2、GPS值赋值给result
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
int ResetOutputLoction(GNSS_t* pgnss, PDR_t* g_pdr, EKFPara_t* kf, LctFs_t* result);
/**----------------------------------------------------------------------
* Function : EkfGnssInsLocFusion
* Description : 采用EKF对GNSS和INS融合定位
* Date : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void EkfGnssInsLocFusion(GNSS_t* pgnss, PDR_t* g_pdr, Classifer_t* sys, double yaw_bias,
EKFPara_t* kf, LctFs_t* res);
/**----------------------------------------------------------------------
* Function : PDRInitialAlignment
* Description : PDR初始对准
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
int GnssInsInitialAlignment(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, LctFs_t* result);
#ifdef __cplusplus
}

13
1.Software/PDR 1.04/include/pdr_api.h
View File

@ -44,17 +44,12 @@ typedef struct _PosFusion{
void Algorithm_Init(void);
/**---------------------------------------------------------------------
* Function : LocationMainLoop
* Function : PDRLocationMainLoop
* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置
* 迹平滑窗口的初始化
* Input : ss_data， 传感器数据
nmea_data, NMEA数据
result，定位结果
* Output : int, 输出标志位
* Date : 2020/7/3 logzhan
* Date : 2022/10/16 logzhan
*---------------------------------------------------------------------**/
int LocationMainLoop(IMU_t* ImuData, Nmea_t* NmeaData, LctFs_t* LocFusion,
FILE *fp_gps);
int PDRLocationMainLoop(IMU_t* ImuData, Nmea_t* NmeaData, LctFs_t* LocFusion);
/**---------------------------------------------------------------------
* Function : pdr_locationMainLoopStr
@ -63,7 +58,7 @@ int LocationMainLoop(IMU_t* ImuData, Nmea_t* NmeaData, LctFs_t* LocFusion,
* Date : 2020/07/03 logzhan
* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑
*---------------------------------------------------------------------**/
int ParseLineAndUpdate(char* line, LctFs_t* result);
int ParseDataAndUpdate(char* line, LctFs_t* result);
/**---------------------------------------------------------------------
* Function : pdr_initRmc

39
1.Software/PDR 1.04/include/pdr_base.h
View File

@ -128,7 +128,7 @@ typedef struct{
typedef struct PDR {
uint32_t status; // PDR当前状态
uint32_t motionType; // 用户运动类型
uint32_t MotionType; // 用户运动类型
uint32_t NoGnssCount; // 没有GNSS信息次数
pdrStatus sysStatus; // PDR系统状态
int sceneType; // 场景类型：1：开阔区域，0：非开阔区域(信号较弱)
@ -189,7 +189,7 @@ PDR_t* Base_Init(void);
* Date : 2020/7/21
*
*---------------------------------------------------------------------**/
void pdr_resetData(void);
void ResetPDRData(void);
/**----------------------------------------------------------------------
* Function : pdr_computePCA
@ -206,8 +206,6 @@ void ComputePCA(AHRS_t* ahrs);
void DetectorUpdate(Detector_t* detector);
void gpsUpdateCb(GNSS_t* gps);
/**************************************************************************
* Description : 计算每步所耗时间
* Input : stepPredict， 步数预测结构体
@ -234,16 +232,6 @@ int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_
*---------------------------------------------------------------------**/
void StateRecognition(float *acc, Classifer_t *sys);
/**************************************************************************
* Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向
* Input : g_pdr，PDR结构体
sys，运动幅度数据结构体
gps_yaw，GPS轨迹角
G_yaw，GPS方向角
ss_data， 传感器数据结构体
* Output : double，用户行走方向
**************************************************************************/
double calPredAngle(PDR_t *g_pdr, Classifer_t *sys, double gps_yaw, double G_yaw, IMU_t *ss_data);
/**************************************************************************
* Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向
@ -258,7 +246,7 @@ double calPredAngle(PDR_t *g_pdr, Classifer_t *sys, double gps_yaw, double G_yaw
kf，EKF结构体
pdr, PDR预测位置成功标志位
**************************************************************************/
void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict,IMU_t *ss_data,
void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict,IMU_t *ss_data,
GNSS_t *pgnss, EKFPara_t *kf);
/**----------------------------------------------------------------------
@ -268,25 +256,6 @@ void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict,IMU_t *ss_data,
*---------------------------------------------------------------------**/
int DetUserStatic(PDR_t *g_pdr, GNSS_t *pgnss, unsigned long delSteps);
/**----------------------------------------------------------------------
* Function : OutputRawGnssPos
* Description : 输出GPS位置
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void OutputRawGnssPos(GNSS_t *pgnss, LctFs_t *result);
/**----------------------------------------------------------------------
* Function : resetOutputResult
* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数
* 2、GPS值赋值给result
* pgnss，GPS数据结构体
* plla_init，待重置NED坐标系原点经纬度
* x_init，y_init待重置NED坐标系原点的n和e坐标
* kf， EKF数据结构体
* Date : 2020/07/08 logzhan
*---------------------------------------------------------------------**/
int ResetOutputResult(GNSS_t *pgnss, PDR_t* g_pdr, EKFPara_t *kf, LctFs_t *result);
/**----------------------------------------------------------------------
* Function : detIsCarMode
* Description : 识别是否是车载模式
@ -317,7 +286,7 @@ int DetectPdrToReset(double pdr_angle, int *gpscnt, unsigned long deltsteps, PDR
* Output : int，初始化标志位
result，定位结果结构体
**************************************************************************/
int InitProc(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr, LctFs_t *result);
int GnssInsInitialAlignment(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr, LctFs_t *result);
/**************************************************************************
* Description : 设置EKF滤波器的Q和R

59
1.Software/PDR 1.04/src/Detector.c
View File

@ -34,17 +34,12 @@
#define INC_PTR(buf, ptr) (((ptr) + 1) % ((buf)->length + 1))
#define DEC_PTR(buf, ptr) (((ptr) + (buf)->length) % ((buf)->length + 1))
#define COST(i, j) cost[(i) + (j) * number]
#define CALIBRATE_ON 1
#define CALIBRATE_OFF 0
extern int debugCount;
static float ellipsoidPoint[3];
static float ellipsoidScale[3];
/* Global Variable Definition ------------------------------------------------*/
static Detector_t g_detector;
static Detector_t pDetector;
static BUFFER_LONG g_mol_buf[2];
static Filter_t g_mol_flt[2];
static Filter_t g_mol_flt[2];
const double g_mol_flt_b[] = { 4.96135120696701e-05, 0.000496135120696701, 0.00223260804313515, 0.00595362144836041,
0.0104188375346307, 0.0125026050415569, 0.0104188375346307, 0.00595362144836041, 0.00223260804313515,
@ -59,10 +54,7 @@ static BUFFER_SHORT g_acc_tmp_buf[3];
BUFFER_SHORT g_gyr_amp_buf[3];
static BUFFER_SHORT g_gyr_tmp_buf[3];
static float mag_buff[MAG_BUF_LEN][3];
static int mag_count;
static unsigned char mag_calibrate;
//static double mag_time;
static int g_label[5] = { 0 };
/* Function Definition -------------------------------------------------------------------------- */
@ -73,7 +65,7 @@ static int g_label[5] = { 0 };
* Date : 2020/02/16 logzhan
*---------------------------------------------------------------------**/
Detector_t* GetDetectorObj(void) {
return &g_detector;
return &pDetector;
}
/**---------------------------------------------------------------------
@ -83,7 +75,7 @@ Detector_t* GetDetectorObj(void) {
*---------------------------------------------------------------------**/
Detector_t* Detector_Init(void) {
DetectorReset();
return &g_detector;
return &pDetector;
}
/**---------------------------------------------------------------------
@ -93,40 +85,39 @@ Detector_t* Detector_Init(void) {
*---------------------------------------------------------------------**/
void DetectorReset(void) {
g_detector.tick = 0;
g_detector.lastType = DETECTOR_TYPE_STATIC;
pDetector.tick = 0;
pDetector.lastType = DETECTOR_TYPE_STATIC;
BufferInit((Buffer_t *)&g_mol_buf[ACCE], "acce_mold_filtered", BUFFER_TYPE_QUEUE, FLT_BUF_LEN);
BufferInit((Buffer_t *)&g_mol_buf[GYRO], "gyro_mold_filtered", BUFFER_TYPE_QUEUE, FLT_BUF_LEN);
BufferInit((Buffer_t *)&g_acc_frq_buf[0], "acc_frq_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
BufferInit((Buffer_t *)&g_acc_frq_buf[1], "acc_frq_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
BufferInit((Buffer_t *)&g_acc_frq_buf[2], "acc_frq_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
BufferInit((Buffer_t *)&g_acc_amp_buf[0], "acc_amp_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
BufferInit((Buffer_t *)&g_acc_amp_buf[1], "acc_amp_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
BufferInit((Buffer_t *)&g_acc_amp_buf[2], "acc_amp_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
BufferInit((Buffer_t *)&g_acc_tmp_buf[0], "acc_tmp_buf_0", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
BufferInit((Buffer_t *)&g_acc_tmp_buf[1], "acc_tmp_buf_1", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
BufferInit((Buffer_t *)&g_acc_tmp_buf[2], "acc_tmp_buf_2", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
BufferInit((Buffer_t *)&g_gyr_frq_buf[0], "gyr_frq_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
BufferInit((Buffer_t *)&g_gyr_frq_buf[1], "gyr_frq_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
BufferInit((Buffer_t *)&g_gyr_frq_buf[2], "gyr_frq_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
BufferInit((Buffer_t *)&g_gyr_amp_buf[0], "gyr_amp_buf_0", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
BufferInit((Buffer_t *)&g_gyr_amp_buf[1], "gyr_amp_buf_1", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
BufferInit((Buffer_t *)&g_gyr_amp_buf[2], "gyr_amp_buf_2", BUFFER_TYPE_QUEUE, FRQ_BUF_LEN);
BufferInit((Buffer_t *)&g_gyr_tmp_buf[0], "gyr_tmp_buf_0", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
BufferInit((Buffer_t *)&g_gyr_tmp_buf[1], "gyr_tmp_buf_1", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
BufferInit((Buffer_t *)&g_gyr_tmp_buf[2], "gyr_tmp_buf_2", BUFFER_TYPE_QUEUE, TMP_BUF_LEN);
FilterSetCoef(&g_mol_flt[ACCE], ARR_LEN(g_mol_flt_b), (double *)g_mol_flt_b, (double *)g_mol_flt_a, (double)g_mol_flt_h0);
FilterSetCoef(&g_mol_flt[GYRO], ARR_LEN(g_mol_flt_b), (double *)g_mol_flt_b, (double *)g_mol_flt_a, (double)g_mol_flt_h0);
memset(*mag_buff,0,sizeof(mag_buff));
mag_count = 0;
mag_calibrate = CALIBRATE_OFF;
ellipsoidPoint[0] = 0;
ellipsoidPoint[1] = 0;
ellipsoidPoint[2] = 0;
ellipsoidScale[0] = 1;
ellipsoidScale[1] = 1;
ellipsoidScale[2] = 1;
memset((void *)g_label, 0, sizeof(g_label));
}
@ -144,16 +135,16 @@ void DetectorUpdateIMU(IMU_t* imu) {
BufferPush((Buffer_t *)&g_mol_buf[GYRO], (float)FilterFilter(&g_mol_flt[GYRO], GET_MOL(imu->gyr.s)));
// g_tick_d 实际上是1.28s检测一次，不然检测的频率过高
if ((g_detector.tick % (int)(1.28 * IMU_SAMPLING_FREQUENCY)) == 0) {
if ((pDetector.tick % (int)(1.28 * IMU_SAMPLING_FREQUENCY)) == 0) {
// 检测用户的运动类型
int detectResult = DetectMotionType();
if (detectResult == DETECTOR_NO_ERROR && g_detector.lastType != g_detector.type) {
DetectorUpdate(&g_detector);
g_detector.lastType = g_detector.type;
if (detectResult == DETECTOR_NO_ERROR && pDetector.lastType != pDetector.type) {
DetectorUpdate(&pDetector);
pDetector.lastType = pDetector.type;
}
}
++g_detector.tick;
++pDetector.tick;
}
/**---------------------------------------------------------------------
@ -654,8 +645,8 @@ int DetectMotionType() {
if (DETECTOR_NO_ERROR != (ret = fft_buffer(s_fft, (Buffer_t *)&g_mol_buf[GYRO]))) {
return ret;
}
if (DETECTOR_NO_ERROR != (ret = find_peaks(frq, amp, 3, s_fft, 1024, 5, (DETECTOR_TYPE_SWINGING == g_detector.type ||
DETECTOR_TYPE_HANDHELD == g_detector.type) ? 20.0f : 100.0f))) {
if (DETECTOR_NO_ERROR != (ret = find_peaks(frq, amp, 3, s_fft, 1024, 5, (DETECTOR_TYPE_SWINGING == pDetector.type ||
DETECTOR_TYPE_HANDHELD == pDetector.type) ? 20.0f : 100.0f))) {
return ret;
}
if (DETECTOR_NO_ERROR != (ret = track_frequency(g_gyr_frq_buf, g_gyr_amp_buf, g_gyr_tmp_buf, frq, amp, 3))) {
@ -664,14 +655,14 @@ int DetectMotionType() {
get_feature(s_feature, 5);
g_detector.type = pdr_detectorPredict(s_feature);
pDetector.type = pdr_detectorPredict(s_feature);
/* debounce */
for (i = 0; i < ARR_LEN(g_label) - 1; ++i) {
g_label[i] = g_label[i + 1];
}
g_label[i] = g_detector.type;
g_detector.type = debounce(g_label, ARR_LEN(g_label));
g_label[i] = pDetector.type;
pDetector.type = debounce(g_label, ARR_LEN(g_label));
return DETECTOR_NO_ERROR;
}

88
1.Software/PDR 1.04/src/Interface.c
View File

@ -20,8 +20,8 @@
const char* PDR_Version = "2.0";
Sensor_t SensorDataHist[IMU_LAST_COUNT] = {{0}};
LatLng_t llaLast = { 0 };
Nmea_t NmeaData;
IMU_t ImuData;
Nmea_t Nmea;
IMU_t Imu;
/* Extern Variable Definition ------------------------------------------------*/
extern EKFPara_t g_kfPara;
@ -99,109 +99,87 @@ void ClearNmeaFlg(Nmea_t * ln){
*---------------------------------------------------------------------**/
void Algorithm_Init(void) {
// 导航相关初始化
PDRNav_Init();
PDR_Init();
// 计步器初始化
Pedometer_Init();
// 初始化sensor历史缓存
memset(SensorDataHist, 0, sizeof(SensorDataHist));
memset(&NmeaData, 0, sizeof(NmeaData));
memset(&ImuData, 0, sizeof(ImuData));
memset(&Nmea, 0, sizeof(Nmea));
memset(&Imu, 0, sizeof(Imu));
}
/**---------------------------------------------------------------------
* Function : ParseLineAndUpdate
* Function : ParseDataAndUpdate
* Description : 解析字符串信息并更新
* Date : 2020/07/03 logzhan
* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑
* Date : 2022/10/16 logzhan
*---------------------------------------------------------------------**/
int ParseLineAndUpdate(char* line, LctFs_t* locFusion) {
ParseLineStr(line, &ImuData, &NmeaData);
return LocationMainLoop(&ImuData, &NmeaData, locFusion, NULL);
int ParseDataAndUpdate(char* line, LctFs_t* LocFusion)
{
ParseLineStr(line, &Imu, &Nmea);
// 给定Imu和Nmea结构体，返回融合位置
return PDRLocationMainLoop(&Imu, &Nmea, LocFusion);
}
/**---------------------------------------------------------------------
* Function : LocationMainLoop
* Function : PDRLocationMainLoop
* Description : PDR定位主循环，主要处理来自上层输入的传感器数据并解算位置
* 迹平滑窗口的初始化
* Date : 2020/07/03 logzhan
* 2020/02/02 logzhan : 增加宏控制是否开启输出平滑
*---------------------------------------------------------------------**/
int LocationMainLoop(IMU_t *ImuData, Nmea_t *NmeaData, LctFs_t *LocFusion, FILE *fpGps) {
int PDRLocationMainLoop(IMU_t *imu, Nmea_t *nmea, LctFs_t *LocFusion) {
int type = 0;
if (ImuData->gyr.update) {
if (imu->gyr.update) {
// 如果陀螺仪更新则采用惯性传感器计算
InsLocation(ImuData,&g_kfPara);
ImuData->gyr.update = UN_UPDATE;
InsLocation(imu,&g_kfPara);
imu->gyr.update = UN_UPDATE;
}
// 如果GPS不更新，返回
if (!NmeaData->update)return TYPE_FIX_NONE;
if (!nmea->update)return TYPE_FIX_NONE;
// 写GPS相关LOG信息
SaveGnssInfo(NmeaData, LocFusion, fpGps);
SaveGnssInfo(nmea, LocFusion, NULL);
// 有GPS则采用GPS融合定位
int flag = GnssInsFusionLocation(NmeaData, &g_kfPara, LocFusion);
int flag = GnssInsFusionLocation(nmea, &g_kfPara, LocFusion);
if (flag != TYPE_FIX_NONE) {
// 如果是开车这种，直接输出GPS，不进行平滑处理
if (flag == TYPE_FIX_GPS) {
LocFusion->latitude = R2D(LocFusion->latitude);
LocFusion->longitudinal = R2D(LocFusion->longitudinal);
LocFusion->latitude = R2D(LocFusion->latitude);
LocFusion->longitudinal = R2D(LocFusion->longitudinal);
} else {
LatLng_t lla = { 0 };
lla.lat = R2D(LocFusion->latitude);
lla.lon = R2D(LocFusion->longitudinal);
if (LocFusion->last_lat != 0.0 && LocFusion->last_lon != 0.0 &&
CalDistance(lla, llaLast) > 10) {
}
LocFusion->latitude = R2D(LocFusion->latitude);
LocFusion->longitudinal = R2D(LocFusion->longitudinal);
LocFusion->latitude = (LocFusion->latitude) * RADIAN_PER_DEG;
LocFusion->longitudinal = (LocFusion->longitudinal) * RADIAN_PER_DEG;
LocFusion->latitude = R2D(LocFusion->latitude);
LocFusion->longitudinal = R2D(LocFusion->longitudinal);
LocFusion->last_lat = LocFusion->latitude;
LocFusion->last_lon = LocFusion->longitudinal;
llaLast.lat = lla.lat;
llaLast.lon = lla.lon;
}
LocFusion->last_lat = LocFusion->latitude;
LocFusion->last_lon = LocFusion->longitudinal;
type = 1;
}else if (LocFusion->last_lat != 0.0 && LocFusion->last_lon != 0.0) {
LocFusion->latitude = LocFusion->last_lat;
LocFusion->longitudinal = LocFusion->last_lon;
type = 1;
}
ClearNmeaFlg(NmeaData);
ClearNmeaFlg(nmea);
return type;
}
/**---------------------------------------------------------------------
* Function : pdr_saveGnssInfo
* Function : SaveGnssInfo
* Description : 保存GPS相关信息
* Date : 2020/7/3 logzhan
* Date : 2022/10/16 logzhan
*---------------------------------------------------------------------**/
void SaveGnssInfo(Nmea_t* nmea_data, LctFs_t* result, FILE* fp_gps)
void SaveGnssInfo(Nmea_t* nmea, LctFs_t* result, FILE* fp_gps)
{
NmeaRMC_t rmc = nmea_data->rmc[1];
NmeaRMC_t rmc = nmea->rmc[1];
// 选取更新时间的rmc
if (nmea_data->rmc[0].time > nmea_data->rmc[1].time) {
rmc = nmea_data->rmc[0];
if (nmea->rmc[0].time > nmea->rmc[1].time) {
rmc = nmea->rmc[0];
}
if (rmc.update && (rmc.status == POSITIONING_Y)) {
char str[256];
memset(str, 0, sizeof(str));
float accuracy = -1.0f;
if (fabs(rmc.time - nmea_data->accuracy.time) < 500){
accuracy = nmea_data->accuracy.err;
if (fabs(rmc.time - nmea->accuracy.time) < 500){
accuracy = nmea->accuracy.err;
}
sprintf(str, "%f,%.10f,%d,%.10f,%d,%.10f,%f,%f", rmc.time, rmc.latitude,
rmc.rmc_latitude_ns,rmc.longitudinal, rmc.longitudinal_ew, rmc.cog, accuracy,

143
1.Software/PDR 1.04/src/Kalman.c
View File

@ -27,13 +27,13 @@ static double EkfMatBuf[7][N][N] = { { {0.0} } };
extern double GpsPos[HIST_GPS_NUM][3];
extern int debugCount;
extern double angle_mean[3];
static int hold_type;
static double b_timestamp;
static double b_last_time;
static double attitude_yaw;
static Detector_t *g_detector;
static Detector_t *pDetector;
static int hold_type;
static double BUL_duration;
static double FUR_duration;
@ -76,7 +76,7 @@ void EKFStatePredict(EKFPara_t *ekf, double stepLen, PDR_t* pdr, int step) {
// angle = g_pdr->heading;
//}
if (pdr->motionType != PDR_MOTION_TYPE_HANDHELD) {
if (pdr->MotionType != PDR_MOTION_TYPE_HANDHELD) {
deltaHeading = 0;
}
@ -503,94 +503,6 @@ void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict, IMU_t *ss_data,
}
}
/**----------------------------------------------------------------------
* Function : ResetOutputResult
* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数
* 2、GPS值赋值给result
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
int ResetOutputResult(GNSS_t *pgnss, PDR_t* g_pdr, EKFPara_t *kf, LctFs_t *result) {
double stepLength = 0.7;
double ned[3] = { 0 };
g_pdr->pllaInit[0] = pgnss->lat;
g_pdr->pllaInit[1] = pgnss->lon;
g_pdr->pllaInit[2] = 0;
WGS842NED(g_pdr->pllaInit, g_pdr->pllaInit, ned);
g_pdr->x0 = ned[0];
g_pdr->y0 = ned[1];
kf->pXk[0] = ned[0];
kf->pXk[1] = ned[1];
kf->pXk[2] = stepLength;
// 重置输出GPS时，GPS的航向角不一定准
kf->pXk[3] = pgnss->yaw*RADIAN_PER_DEG;
for (uchar i = 0; i < N; i++) {
kf->Xk[i] = kf->pXk[i];
}
// 清除卡尔曼Q矩阵和R矩阵
for (int i = 0; i < N; i++) {
for (int l = 0; l < N; l++) {
kf->Q[i][l] = 0.0;
kf->R[i][l] = 0.0;
}
}
// 输出GPS位置结果
OutputRawGnssPos(pgnss, result);
g_pdr->NoGnssCount = 0;
return PDR_TRUE;
}
int InitProc(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr,LctFs_t *result)
{
double stepLen = 0.7; // 初始运动步长为0.7m
if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.5 && pgnss->yaw != -1) {
// plla 类似位置坐标的原点(单位是经纬度)
g_pdr->pllaInit[0] = pgnss->lat;
g_pdr->pllaInit[1] = pgnss->lon;
g_pdr->pllaInit[2] = 0;
// plla 转 ned
double ned[3] = { 0 };
WGS842NED(g_pdr->pllaInit, g_pdr->pllaInit, ned);
// 在NED坐标系的计算, 单位是米
g_pdr->x0 = ned[0];
g_pdr->y0 = ned[1];
// 初始化卡尔曼滤波器的观测量
kf->pXk[0] = ned[0]; // 状态量1: 北向x
kf->pXk[1] = ned[1]; // 状态量2：东向y
kf->pXk[2] = stepLen; // 状态量3：步长
kf->pXk[3] = pgnss->yaw * RADIAN_PER_DEG; // 状态量4：航向角
for (uchar i = 0; i < N; i++) {
kf->Xk[i] = kf->pXk[i];
}
kf->initHeading = kf->pXk[3];
for (int i = 0; i < N; i++) {
for (int l = 0; l < N; l++) {
kf->Q[i][l] = 0.0;
kf->R[i][l] = 0.0;
}
}
OutputRawGnssPos(pgnss, result);
g_pdr->sysStatus = IS_NORMAL;
return PDR_TRUE;
}
if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel > 0 && pgnss->yaw != -1) {
OutputRawGnssPos(pgnss, result);
return PDR_TRUE;
}
return PDR_FALSE;
}
/**----------------------------------------------------------------------
* Function : EKFCalQRMatrix
@ -602,8 +514,8 @@ int InitProc(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr,LctFs_t *result)
void EKFCalQRMatrix(Nmea_t *nmea, PDR_t *g_pdr, Classifer_t *sys,
EKFPara_t *kf) {
if (g_pdr->motionType != PDR_MOTION_TYPE_HANDHELD &&
g_pdr->motionType != PDR_MOTION_TYPE_STATIC) {
if (g_pdr->MotionType != PDR_MOTION_TYPE_HANDHELD &&
g_pdr->MotionType != PDR_MOTION_TYPE_STATIC) {
kf->Q[0][0] = 25; // PDRÔ¤²âλÖÃÔëÉù
kf->Q[1][1] = 25; // PDRÔ¤²âλÖÃÔëÉù
kf->Q[2][2] = 1; // PDR²½³¤ÔëÉù
@ -641,49 +553,4 @@ void EKFCalQRMatrix(Nmea_t *nmea, PDR_t *g_pdr, Classifer_t *sys,
kf->R[3][3] = 10000;
}
}
/**----------------------------------------------------------------------
* Function : GnssInsLocFusion
* Description : PDR的GNSS和INS融合定位
* Date : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void GnssInsLocFusion(GNSS_t *pgnss, PDR_t *pdr, Classifer_t *sys, double yaw_bias,
EKFPara_t *kf, LctFs_t *res) {
double plla[3] = { 0 };
double Ned[3] = { 0 };
double yaw_thr = 6;
int scene_type = pdr->sceneType;
plla[0] = pgnss->lat;
plla[1] = pgnss->lon;
plla[2] = 0;
WGS842NED(plla, pdr->pllaInit, Ned);
pgnss->xNed = Ned[0];
pgnss->yNed = Ned[1];
//调整融合策略
EKFStateUpdate(kf, pgnss, sys, pdr, scene_type);
for (uchar i = 0; i < N; i++) {
for (uchar l = 0; l < N; l++) {
kf->pPk[i][l] = kf->Pk[i][l];
}
kf->pXk[i] = kf->Xk[i];
}
Ned[0] = kf -> pXk[0] - pdr->x0;
Ned[1] = kf -> pXk[1] - pdr->y0;
Ned[2] = 0;
NED2WGS84(pdr->pllaInit, Ned, plla);
res->latitude = plla[0];
res->latitude_ns = pgnss->lat_ns;
res->longitudinal = plla[1];
res->longitudinal_ew = pgnss->lon_ew;
res->time = pgnss->timestamps;
pdr->NoGnssCount = 0;
}

208
1.Software/PDR 1.04/src/Location.c
View File

@ -43,7 +43,7 @@ float bias_dir[2] = { 0 };
* Description : PDR导航系统初始化
* Date : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void PDRNav_Init(void) {
void PDR_Init(void) {
memset(&g_kfPara, 0, sizeof(g_kfPara));
memset(&pgnss, 0, sizeof(pgnss));
@ -112,7 +112,7 @@ int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) {
pdr->gyroTime = ImuData->gyr.time;
// 为什么手持类型是0.1的运动频率？
if (pdr->motionType == DETECTOR_TYPE_HANDHELD)pdr->motionFreq = 0.1f;
if (pdr->MotionType == DETECTOR_TYPE_HANDHELD)pdr->motionFreq = 0.1f;
// 计步器输入传感器数据，占10%的运行时间
PedometerUpdate(ImuData, &pdr->steps);
@ -163,15 +163,14 @@ int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) {
* Description : PDR GPS融合INS惯性定位
* Date : 2021/01/29 logzhan
*---------------------------------------------------------------------**/
int GnssInsFusionLocation(Nmea_t *nmea_data, EKFPara_t *kf, LctFs_t *result)
int GnssInsFusionLocation(Nmea_t *nmea, EKFPara_t *kf, LctFs_t *result)
{
// GPS数据更新
GnssUpdate(&pgnss, nmea_data);
Nmea2Gnss(nmea, &pgnss);
// 检查时间差是否满足条件
if (fabs(pgnss.timestamps - pdr->ts) >= 1000)return TYPE_FIX_NONE;
// 根据nmea数据识别场景是否是开阔天空场景
pdr->sceneType = isOpenArea(nmea_data);
pdr->sceneType = isOpenArea(nmea);
// 根据HDOP、速度、卫星数量等判断gps yaw是否可用
pdr->gpsSpeed = pgnss.vel * GPS_SPEED_UNIT;
@ -183,12 +182,13 @@ int GnssInsFusionLocation(Nmea_t *nmea_data, EKFPara_t *kf, LctFs_t *result)
result->pdrHeading = kf->pXk[3];
result->gpsHeading = pdr->gpsHeading;
result->hdop = pgnss.HDOP;
result->accuracy = nmea_data->accuracy.err;
result->accuracy = nmea->accuracy.err;
result->gpsSpeed = pdr->gpsSpeed;
result->motionType = pdr->motionType;
result->motionType = pdr->MotionType;
#endif
CalPdrHeadingOffset(nmea_data, pdr);
// 利用Nmea信息修正Ins航向角
CalPdrHeadingOffset(nmea, pdr);
// 计算两次GPS时间之间，步数变化量
pdr->deltaStepsPerGps = pdr->steps - pdr->lastGpsSteps;
@ -199,16 +199,22 @@ int GnssInsFusionLocation(Nmea_t *nmea_data, EKFPara_t *kf, LctFs_t *result)
//根据历史GPS拟合输出预估的GPS轨迹航向角
pdr->trackHeading = CalGnssTrackHeading(GpsPos, pgnss);
// 如果系统没有初始化，则先初始化
if (pdr->sysStatus == IS_INITIAL) {
return InitProc(&pgnss, kf, pdr, result);
// Gnss系统初始对准
return GnssInsInitialAlignment(&pgnss, kf, pdr, result);
}
// 如果惯导没有达到输出条件，则不预测轨迹
if (!pdr->fusionPdrFlg)return ResetOutputResult(&pgnss, pdr, kf, result);
if (!pdr->fusionPdrFlg)
{
return ResetOutputLoction(&pgnss, pdr, kf, result);
}
// 根据GPS信息, 计算kf的过程噪声和观测噪声
EKFCalQRMatrix(nmea_data, pdr, &sys, kf);
EKFCalQRMatrix(nmea, pdr, &sys, kf);
// 运行卡尔曼位置融合
if (pgnss.satellites_num > 4){
// 如果GPS航向角处于无效状态且速度也是无效状态
if (pgnss.yaw == INVAILD_GPS_YAW || pgnss.vel <= 0.0) {
@ -220,14 +226,14 @@ int GnssInsFusionLocation(Nmea_t *nmea_data, EKFPara_t *kf, LctFs_t *result)
pgnss.yaw = (float)kf->pXk[3];
}
if (pgnss.yaw != INVAILD_GPS_YAW) {
GnssInsLocFusion(&pgnss, pdr, &sys, yaw_bias, kf, result);
EkfGnssInsLocFusion(&pgnss, pdr, &sys, yaw_bias, kf, result);
return TYPE_FIX_PDR;
}
}
// 不满足推算条件，直接输出原始GPS
if (pgnss.satellites_num > 4 && nmea_data->accuracy.err > 0 &&
nmea_data->accuracy.err < 15) {
if (pgnss.satellites_num > 4 && nmea->accuracy.err > 0 &&
nmea->accuracy.err < 15) {
OutputRawGnssPos(&pgnss, result);
pdr->NoGnssCount = 0;
return TYPE_FIX_GPS;
@ -276,11 +282,12 @@ void CalPdrHeadingOffset(Nmea_t* nmea_data, PDR_t* p_pdr) {
if ((pgnss.satellites_num > 4 && pgnss.HDOP < 2.0 && pgnss.yaw != -1 && pgnss.vel >= 1.0)
&& (nmea_data->accuracy.err > 0 && nmea_data->accuracy.err < 4)
&& (DetUserStatic(pdr, &pgnss, (p_pdr->steps - p_pdr->lastSteps)) == 0 && pdr->motionType == 2))
&& (DetUserStatic(pdr, &pgnss, (p_pdr->steps - p_pdr->lastSteps)) == 0 && pdr->MotionType == 2))
{
/*这里放姿态角与GPS对准part1 start*/
double imuHeading = atan2(2 * (g_ahrs.qnb[0] * g_ahrs.qnb[3] + g_ahrs.qnb[1] * g_ahrs.qnb[2]),
1 - 2 * (g_ahrs.qnb[2] * g_ahrs.qnb[2] + g_ahrs.qnb[3] * g_ahrs.qnb[3]));
if (imuHeading < 0) {
imuHeading += TWO_PI;
}
@ -294,15 +301,15 @@ void CalPdrHeadingOffset(Nmea_t* nmea_data, PDR_t* p_pdr) {
}
/**----------------------------------------------------------------------
* Function : pdr_detectFixMode
* Function : DetectFixMode
* Description : 检测当前PDR处于的模式，如果是车载和静止模式,根据情况选择输出
* 原始GPS或者不输出
* Date : 2020/07/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高
* 代码的可读性
*---------------------------------------------------------------------**/
int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, LctFs_t* result) {
int DetectFixMode(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* pdr, LctFs_t* result) {
// 检测用户是否处于静止状态
if (DetUserStatic(pdr, pgnss, g_pdr->deltaStepsPerGps)) {
if (DetUserStatic(pdr, pgnss, pdr->deltaStepsPerGps)) {
if (pgnss->error > 0 && pgnss->error < 15 &&
pgnss->lastError >= pgnss->error) {
OutputRawGnssPos(pgnss, result);
@ -372,17 +379,158 @@ void Nmea2Gnss(Nmea_t* nmea_data, GNSS_t* pgnss)
pgnss->satellites_num = nmea_data->gga.satellites_nb;
}
/**----------------------------------------------------------------------
* Function : GnssUpdate
* Description : nmea数据结构转gnss数据
* Date : 2020/07/08 logzhan
* 2020/02/09 logzhan : 函数对应原pdr_gpsUpdate函数，
* 经过引用分析发现每次GPS更新拷贝gnss结构体和nmea结构体其实没
* 什么用，还增加运算量
* Function : OutputRawGnssPos
* Description : 输出GPS位置
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void GnssUpdate(GNSS_t* gps, Nmea_t* nmea) {
// nmea数据转gnss数据结构
Nmea2Gnss(nmea, &pgnss);
// GPS回调更新，暂时不清楚函数功能
gpsUpdateCb(gps);
void OutputRawGnssPos(GNSS_t* pgnss, LctFs_t* result) {
result->latitude = pgnss->lat;
result->latitude_ns = pgnss->lat_ns;
result->longitudinal = pgnss->lon;
result->longitudinal_ew = pgnss->lon_ew;
result->time = pgnss->timestamps;
}
/**----------------------------------------------------------------------
* Function : ResetOutputLoction
* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数
* 2、GPS值赋值给result
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
int ResetOutputLoction(GNSS_t* pgnss, PDR_t* g_pdr, EKFPara_t* kf, LctFs_t* result) {
double stepLength = 0.7;
double ned[3] = { 0 };
g_pdr->pllaInit[0] = pgnss->lat;
g_pdr->pllaInit[1] = pgnss->lon;
g_pdr->pllaInit[2] = 0;
WGS842NED(g_pdr->pllaInit, g_pdr->pllaInit, ned);
g_pdr->x0 = ned[0];
g_pdr->y0 = ned[1];
kf->pXk[0] = ned[0];
kf->pXk[1] = ned[1];
kf->pXk[2] = stepLength;
// 重置输出GPS时，GPS的航向角不一定准
kf->pXk[3] = pgnss->yaw * RADIAN_PER_DEG;
for (uchar i = 0; i < N; i++) {
kf->Xk[i] = kf->pXk[i];
}
// 清除卡尔曼Q矩阵和R矩阵
for (int i = 0; i < N; i++) {
for (int l = 0; l < N; l++) {
kf->Q[i][l] = 0.0;
kf->R[i][l] = 0.0;
}
}
// 输出GPS位置结果
OutputRawGnssPos(pgnss, result);
g_pdr->NoGnssCount = 0;
return PDR_TRUE;
}
/**----------------------------------------------------------------------
* Function : EkfGnssInsLocFusion
* Description : 采用EKF对GNSS和INS融合定位
* Date : 2022/09/21 logzhan
*---------------------------------------------------------------------**/
void EkfGnssInsLocFusion(GNSS_t* pgnss, PDR_t* pdr, Classifer_t* sys, double yaw_bias,
EKFPara_t* kf, LctFs_t* res) {
double plla[3] = { 0 };
double Ned[3] = { 0 };
double yaw_thr = 6;
int scene_type = pdr->sceneType;
plla[0] = pgnss->lat;
plla[1] = pgnss->lon;
plla[2] = 0;
WGS842NED(plla, pdr->pllaInit, Ned);
pgnss->xNed = Ned[0];
pgnss->yNed = Ned[1];
//调整融合策略
EKFStateUpdate(kf, pgnss, sys, pdr, scene_type);
for (uchar i = 0; i < N; i++) {
for (uchar l = 0; l < N; l++) {
kf->pPk[i][l] = kf->Pk[i][l];
}
kf->pXk[i] = kf->Xk[i];
}
Ned[0] = kf->pXk[0] - pdr->x0;
Ned[1] = kf->pXk[1] - pdr->y0;
Ned[2] = 0;
NED2WGS84(pdr->pllaInit, Ned, plla);
res->latitude = plla[0];
res->latitude_ns = pgnss->lat_ns;
res->longitudinal = plla[1];
res->longitudinal_ew = pgnss->lon_ew;
res->time = pgnss->timestamps;
pdr->NoGnssCount = 0;
}
/**----------------------------------------------------------------------
* Function : PDRInitialAlignment
* Description : PDR初始对准
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
int GnssInsInitialAlignment(GNSS_t* pgnss, EKFPara_t* kf, PDR_t* g_pdr, LctFs_t* result)
{
double stepLen = 0.7; // 初始运动步长为0.7m
if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.5 && pgnss->yaw != -1) {
// plla 类似位置坐标的原点(单位是经纬度)
g_pdr->pllaInit[0] = pgnss->lat;
g_pdr->pllaInit[1] = pgnss->lon;
g_pdr->pllaInit[2] = 0;
// plla 转 ned
double ned[3] = { 0 };
WGS842NED(g_pdr->pllaInit, g_pdr->pllaInit, ned);
// 在NED坐标系的计算, 单位是米
g_pdr->x0 = ned[0];
g_pdr->y0 = ned[1];
// 初始化卡尔曼滤波器的观测量
kf->pXk[0] = ned[0]; // 状态量1: 北向x
kf->pXk[1] = ned[1]; // 状态量2：东向y
kf->pXk[2] = stepLen; // 状态量3：步长
kf->pXk[3] = pgnss->yaw * RADIAN_PER_DEG; // 状态量4：航向角
for (uchar i = 0; i < N; i++) {
kf->Xk[i] = kf->pXk[i];
}
kf->initHeading = kf->pXk[3];
for (int i = 0; i < N; i++) {
for (int l = 0; l < N; l++) {
kf->Q[i][l] = 0.0;
kf->R[i][l] = 0.0;
}
}
OutputRawGnssPos(pgnss, result);
g_pdr->sysStatus = IS_NORMAL;
return PDR_TRUE;
}
if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel > 0 && pgnss->yaw != -1) {
OutputRawGnssPos(pgnss, result);
return PDR_TRUE;
}
return PDR_FALSE;
}

11
1.Software/PDR 1.04/src/Main.cpp
View File

@ -12,9 +12,6 @@
* 度条显示轨迹。可以对比查看轨迹效果。
*
* 一、优化方向和工作内容
* 1) 要跟品质那边谈高通效果对比版本SAP5.0
* 2）能够兼容老版本，2个月能够更新版本，能够多次迭代测试
* 2021/02/09 logzhan ：发现PDR部分bug记录
* 1) GPS速度实际需要乘以0.51444的，但是除了滤波器计算步频之外，
* 其他地方都是用的实际的GPS速度，包括步数预测
* 2) 惯性导航更新部分buffer mean使用的太多了, 计算平均没有优化
@ -69,7 +66,7 @@ using namespace std;
ResultTracks resTracks;
FILE* SimFile = NULL;
/* Extern Variable Definition ------------------------------------------------*/
extern "C" PDR g_pdr;
extern "C" PDR Pdr;
extern "C" double refGpsYaw;
/**----------------------------------------------------------------------
@ -121,7 +118,7 @@ int main(int argc, char* argv[])
// 解析文本仿真
while (fgets(line, 256, fileFp)){
// 解析文本数据仿真
if (ParseLineAndUpdate(line, &locFusion) != TYPE_FIX_NONE){
if (ParseDataAndUpdate(line, &locFusion) != TYPE_FIX_NONE){
// 更新结果轨迹，用于最后的kml生成
UpdateResTrack(resTracks, locFusion);
}
@ -398,7 +395,7 @@ PosFusion LibPDR_StepUpdateGPS(int useGpsFlg)
char line[256] = { 0 };
while (fgets(line, 256, SimFile)) {
int flag = ParseLineAndUpdate(line, &lct_fusion);
int flag = ParseDataAndUpdate(line, &lct_fusion);
if (flag) {
@ -441,7 +438,7 @@ void setSimulatorFileCsvPath(char* path) {
* Date : 2020/8/3 logzhan
*---------------------------------------------------------------------**/
void setRefGpsYaw() {
refGpsYaw = g_pdr.gpsHeading;
refGpsYaw = Pdr.gpsHeading;
printf("ref Gps Yaw = %f\n", refGpsYaw);
//g_pdr.sysStatus = IS_INITIAL;
//g_pdr.fusionPdrFlg = ON;

38
1.Software/PDR 1.04/src/Munkres.c
View File

@ -50,9 +50,7 @@ int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) {
time_algo_1 = uTimetick_Get();
#endif
while (7 != step) {
#ifdef __MATLAB_DBG
mexPrintf("step %d:\n", step);
#endif
switch (step) {
case 0:
case 1:
@ -66,25 +64,10 @@ int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) {
break;
case 4:
step = step_four(g_cost, g_mark, m, n, &g_row_cover, &g_col_cover, &i, &j);
#if PRINT_ALGO_RUN_TIME
time_algo_3 = uTimetick_Get();
time_algo_3 = (time_algo_3-time_algo_1);
#if defined(LOCATION_PLATFORM_QCOM_MODEM)
Geek_PDR_LOGH_NMEA("step_4 run time ms %d",(int)time_algo_3,0,0,0);
#endif
#endif
break;
case 5:
step = step_five(g_mark, m, n, &g_row_cover, &g_col_cover, i, j);
#if PRINT_ALGO_RUN_TIME
time_algo_4 = uTimetick_Get();
time_algo_4 = (time_algo_4-time_algo_1);
#if defined(LOCATION_PLATFORM_QCOM_MODEM)
Geek_PDR_LOGH_NMEA("step_5 run time ms %d",(int)time_algo_4,0,0,0);
#endif
#endif
break;
case 6:
step = step_six(g_cost, m, n, &g_row_cover, &g_col_cover);
@ -93,26 +76,7 @@ int MUNKRES_get_assignment(int *assignment, float *cost, int m, int n) {
step = 7;
break;
}
#ifdef __MATLAB_DBG
mexPrintf(" cost:\n", step);
display_cost_matrix_and_cover(g_cost, m, n, g_row_cover, g_col_cover);
mexPrintf(" mark:\n", step);
display_mark_matrix_and_cover(g_mark, m, n, g_row_cover, g_col_cover);
#endif
}
#if PRINT_ALGO_RUN_TIME
time_algo_2 = uTimetick_Get();
#endif
#if PRINT_ALGO_RUN_TIME
time_algo_2 = (time_algo_2-time_algo_1);
// time_algo_3 = (time_algo_3-time_algo_2);
#if defined(LOCATION_PLATFORM_QCOM_MODEM)
Geek_PDR_LOGH_NMEA("munkres run time ms %d",(int)time_algo_2,0,0,0);
#endif
// Geek_PDR_LOGH_NMEA("pdr2 run time ms %d %d",(int)time_algo_5,time_algo_6,0,0);
#endif
for (i = 0; i < m; ++i) {
for (j = 0; j < n; ++j) {

545
1.Software/PDR 1.04/src/PDRBase.c
View File

@ -17,357 +17,60 @@
#include "buffer.h"
#include "Filter.h"
#include "pdr_base.h"
#include "Fft.h"
/* Macro Definition ----------------------------------------------------------*/
#define DOWNSAMPLING_RATE 4
#define BUF_LEN 256
#define ARR_LEN(arr) (sizeof(arr) / sizeof(*arr))
#define INC_PTR(buf, ptr) (((ptr) + 1) % ((buf)->length + 1))
#define GET_MOL(v) ((float)sqrt((v)[0] * (v)[0] + (v)[1] * (v)[1]))
#define P_SET_BIT(msk) (g_pdr.status |= PDR_STATUS_##msk)
#define P_CLR_BIT(msk) (g_pdr.status &= ~PDR_STATUS_##msk)
#define P_TST_BIT(msk) (g_pdr.status & PDR_STATUS_##msk)
/* Global Variable Definition ------------------------------------------------*/
PDR_t g_pdr;
static Detector_t *g_detector;
static uint64_t g_tick_p;
static uint32_t g_motion_type;
static uint32_t g_axis_ceof_counter;
static BUFFER_LONG g_axis[9];
static BUFFER_LONG g_grav[2];
static BUFFER_LONG g_erro;
static BUFFER_LONG g_posi[4];
PDR_t Pdr;
static int angle_count;
static double p_angle[3][BUF_LEN];
double angle_mean[3] = { 0 };
static Filter_t g_grv_flt[2];
#if defined(__MATLAB_DBG)
static int g_debug[2];
static float axis_mean[9];
#endif
const double g_grv_flt_b[] = { 4.62344870721942e-10, 4.62344870721942e-09, 2.08055191824874e-08, 5.54813844866331e-08,
9.70924228516079e-08, 1.16510907421929e-07, 9.70924228516079e-08, 5.54813844866331e-08,
2.08055191824874e-08, 4.62344870721942e-09, 4.62344870721942e-10 };
const double g_grv_flt_a[] = { 1, -8.39368255078839, 31.8158646581919, -71.7026569821430, 106.381617694638,
-108.553825650279, 77.1444606240244, -37.6960546719428, 12.1197601392885,
-2.31493776310228, 0.199454975553812 };
const double g_grv_flt_h0 = 1;
PDR_t* Base_Init() {
g_detector = GetDetectorObj();
BufferInit((Buffer_t *)&g_axis[0], "x_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN);
BufferInit((Buffer_t *)&g_axis[1], "x_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN);
BufferInit((Buffer_t *)&g_axis[2], "x_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN);
BufferInit((Buffer_t *)&g_axis[3], "y_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN);
BufferInit((Buffer_t *)&g_axis[4], "y_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN);
BufferInit((Buffer_t *)&g_axis[5], "y_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN);
BufferInit((Buffer_t *)&g_axis[6], "z_axis_0", BUFFER_TYPE_QUEUE, BUF_LEN);
BufferInit((Buffer_t *)&g_axis[7], "z_axis_1", BUFFER_TYPE_QUEUE, BUF_LEN);
BufferInit((Buffer_t *)&g_axis[8], "z_axis_2", BUFFER_TYPE_QUEUE, BUF_LEN);
BufferInit((Buffer_t *)&g_grav[0], "grav_0", BUFFER_TYPE_QUEUE, BUF_LEN);
BufferInit((Buffer_t *)&g_grav[1], "grav_1", BUFFER_TYPE_QUEUE, BUF_LEN);
BufferInit((Buffer_t *)&g_erro, "erro", BUFFER_TYPE_QUEUE, 5 * IMU_SAMPLING_FREQUENCY / DOWNSAMPLING_RATE);
BufferInit((Buffer_t *)&g_posi[0], "posi_0", BUFFER_TYPE_QUEUE, 60);
BufferInit((Buffer_t *)&g_posi[1], "posi_1", BUFFER_TYPE_QUEUE, 60);
BufferInit((Buffer_t *)&g_posi[2], "posi_2", BUFFER_TYPE_QUEUE, 60);
BufferInit((Buffer_t *)&g_posi[3], "posi_3", BUFFER_TYPE_QUEUE, 60);
FilterSetCoef(&g_grv_flt[0], ARR_LEN(g_grv_flt_b), (double *)g_grv_flt_b, (double *)g_grv_flt_a, (double)g_grv_flt_h0);
FilterSetCoef(&g_grv_flt[1], ARR_LEN(g_grv_flt_b), (double *)g_grv_flt_b, (double *)g_grv_flt_a, (double)g_grv_flt_h0);
pdr_resetData();
return &g_pdr;
PDR_t* Base_Init()
{
ResetPDRData();
return &Pdr;
}
/**----------------------------------------------------------------------
* Function : pdr_resetData
* Function : ResetPDRData
* Description : 重置PDR变量
* Date : 2020/7/21 logzhan
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void pdr_resetData(void) {
memset(&g_pdr, 0, sizeof(g_pdr));
g_pdr.status = PDR_STATUS_RESET;
g_pdr.axis_ceofficient[0] = 0;
g_pdr.axis_ceofficient[1] = 1;
g_pdr.axis_ceofficient[2] = 0;
g_pdr.bias_direction[0] = 1;
g_pdr.bias_direction[1] = 0;
g_pdr.pca_accuracy = -1;
g_pdr.bias_accuracy = -1;
void ResetPDRData(void) {
memset(&Pdr, 0, sizeof(Pdr));
Pdr.status = PDR_STATUS_RESET;
Pdr.axis_ceofficient[0] = 0;
Pdr.axis_ceofficient[1] = 1;
Pdr.axis_ceofficient[2] = 0;
Pdr.bias_direction[0] = 1;
Pdr.bias_direction[1] = 0;
Pdr.pca_accuracy = -1;
Pdr.bias_accuracy = -1;
// 初始坐标初始化为0
g_pdr.x0 = 0;
g_pdr.y0 = 0;
Pdr.x0 = 0;
Pdr.y0 = 0;
// 初始步数为0
g_pdr.steps = 0;
g_pdr.lastSteps = 0;
g_pdr.lastGpsSteps = 0;
g_pdr.deltaStepsPerGps = 0;
g_pdr.heading = 0.0;
g_pdr.lastHeading = 0.0;
g_pdr.deltaHeading = 0.0;
g_pdr.ts = 0.0;
g_pdr.sysStatus = IS_INITIAL;
g_pdr.NoGnssCount = 0;
g_pdr.sceneType = UN_OPEN_AREA;
g_pdr.fusionPdrFlg = PDR_FALSE;
g_pdr.gpsHeading = 0;
g_pdr.trackHeading = ITEM_INVALID;
g_pdr.motionFreq = 0.1f;
g_pdr.insHeadingOffset = 0.0f;
g_pdr.gyroTime = 0.0;
memset(g_pdr.pllaInit, 0, sizeof(g_pdr.pllaInit));
Pdr.steps = 0;
Pdr.lastSteps = 0;
Pdr.lastGpsSteps = 0;
Pdr.deltaStepsPerGps = 0;
Pdr.heading = 0.0;
Pdr.lastHeading = 0.0;
Pdr.deltaHeading = 0.0;
Pdr.ts = 0.0;
Pdr.sysStatus = IS_INITIAL;
Pdr.NoGnssCount = 0;
Pdr.sceneType = UN_OPEN_AREA;
Pdr.fusionPdrFlg = PDR_FALSE;
Pdr.gpsHeading = 0;
Pdr.trackHeading = ITEM_INVALID;
Pdr.motionFreq = 0.1f;
Pdr.insHeadingOffset = 0.0f;
Pdr.gyroTime = 0.0;
g_tick_p = 0;
g_motion_type = g_detector->type;
g_axis_ceof_counter = 0;
BufferClear((Buffer_t *)&g_axis[0]);
BufferClear((Buffer_t *)&g_axis[1]);
BufferClear((Buffer_t *)&g_axis[2]);
BufferClear((Buffer_t *)&g_axis[3]);
BufferClear((Buffer_t *)&g_axis[4]);
BufferClear((Buffer_t *)&g_axis[5]);
BufferClear((Buffer_t *)&g_axis[6]);
BufferClear((Buffer_t *)&g_axis[7]);
BufferClear((Buffer_t *)&g_axis[8]);
BufferClear((Buffer_t *)&g_grav[0]);
BufferClear((Buffer_t *)&g_grav[1]);
BufferClear((Buffer_t *)&g_erro);
BufferClear((Buffer_t *)&g_posi[0]);
BufferClear((Buffer_t *)&g_posi[1]);
BufferClear((Buffer_t *)&g_posi[2]);
BufferClear((Buffer_t *)&g_posi[3]);
FilterReset(&g_grv_flt[0]);
FilterReset(&g_grv_flt[1]);
angle_count = 0;
memset(&p_angle, 0, sizeof(p_angle));
memset(&angle_mean, 0, sizeof(angle_mean));
}
static float get_main_frequency(BUFFER_SHORT *frequency, BUFFER_SHORT *amplitude, int number) {
float amp[3] = { {0.0f} };
float frq;
int count;
int index;
int i;
if (number > 3) {
return 0;
}
for (i = 0; i < number; ++i) {
amp[i] = 0;
count = 0;
for (index = amplitude[i]._bottom; index != amplitude[i]._top; index = INC_PTR(&amplitude[i], index)) {
if (0 != amplitude[i].data[index]) {
amp[i] += amplitude[i].data[index];
++count;
}
}
if (0 != count) {
amp[i] /= count;
}
}
i = amp[0] > amp[1] ? 0 : 1;
i = amp[i] > amp[2] ? i : 2;
frq = 0;
count = 0;
for (index = frequency[i]._bottom; index != frequency[i]._top; index = INC_PTR(&frequency[i], index)) {
if (0 != frequency[i].data[index]) {
frq += frequency[i].data[index];
++count;
}
}
if (0 != count) {
frq /= count;
}
return frq;
}
static int get_optimal_length_by_frequency(BUFFER_LONG *buffer, float frequency) {
int temp;
if (0 != frequency) {
temp = (int)((int)(buffer->length / (IMU_SAMPLING_FREQUENCY / frequency)) * (IMU_SAMPLING_FREQUENCY / frequency));
return 0 == temp ? buffer->length : temp;
}
else {
return buffer->length;
}
}
static void get_buffer_mean(float *mean, BUFFER_LONG *buffer, int length) {
int count;
int index;
int i;
count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1);
length = count < length ? count : length;
count = 0;
memset(mean, 0, 3 * sizeof(*mean));
for (index = (buffer->_top + buffer->length + 1 - length) % (buffer->length + 1); index != buffer->_top; index = INC_PTR(buffer, index)) {
for (i = 0; i < 3; ++i) {
mean[i] += buffer[i].data[index];
}
++count;
}
mean[0] /= count;
mean[1] /= -count;
mean[2] /= -count;
}
static void get_pca_direction(float *direction, BUFFER_LONG *buffer, int length) {
float buff_mean[2];
float varX;
float varY;
float varXY;
float lambda;
int count;
int index;
int i;
count = (buffer->_top - buffer->_bottom + buffer->length + 1) % (buffer->length + 1);
length = count < length ? count : length;
memset(buff_mean, 0, sizeof(buff_mean));
for (index = (buffer[0]._top + buffer[0].length + 1 - length) % (buffer[0].length + 1); index != buffer[0]._top; index = INC_PTR(&buffer[0], index)) {
for (i = 0; i < 2; ++i) {
buff_mean[i] += buffer[i].data[index];
}
}
buff_mean[0] /= length;
buff_mean[1] /= length;
varX = 0;
varY = 0;
varXY = 0;
for (index = (buffer[0]._top + buffer[0].length + 1 - length) % (buffer[0].length + 1); index != buffer[0]._top; index = INC_PTR(&buffer[0], index)) {
float deltaX = buffer[0].data[index] - buff_mean[0];
float deltaY = buffer[1].data[index] - buff_mean[1];
varX += deltaX * deltaX;
varY += deltaY * deltaY;
varXY += deltaX * deltaY;
}
lambda = ((varX + varY) + (float)sqrt((varX - varY) * (varX - varY) + 4 * varXY * varXY)) * 0.5f;
if (0 != varXY) {
direction[0] = (lambda - varY) / varXY;
direction[1] = InvSqrt(direction[0] * direction[0] + 1);
direction[0] *= direction[1];
direction[1] = -direction[1];
}
else {
direction[0] = 0;
direction[1] = 0;
}
}
static void caculate_determinant(float *determinant, float A[][3], int row, int col) {
int k = 0;
float temp[4];
for (uchar i = 0; i < 3; ++i) {
if (row == i) continue;
for (uchar j = 0; j < 3; ++j) {
if (col == j) continue;
temp[k++] = A[i][j];
}
}
*determinant = temp[0] * temp[3] - temp[1] * temp[2];
}
static void caculate_adjoint_matrix(float A_star[][3], float A[][3]) {
int i;
int j;
for (i = 0; i < 3; ++i) {
for (j = 0; j < 3; ++j) {
caculate_determinant(&A_star[i][j], A, j, i);
if ((i + j) % 2) {
A_star[i][j] = -A_star[i][j];
}
}
}
}
static void get_ceof_by_pca(float *ceof, float *axis_mean, float *pca_direction) {
float A[3][3];
float A_star[3][3];
float x[3];
float axs_dir[2];
float mold;
int dir;
int i;
int j;
/* caculate adjoint matrix */
for (i = 0; i < 3; ++i) {
for (j = 0; j < 3; ++j) {
A[i][j] = axis_mean[j * 3 + i];
}
}
caculate_adjoint_matrix(A_star, A);
/* caculate x vector (Ax = b) */
for (i = 0; i < 3; ++i) {
x[i] = A_star[i][0] * pca_direction[0] + A_star[i][1] * pca_direction[1];
}
mold = (float)sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]);
if (0 == mold) {
goto GET_CEOF_BY_PCA_ARITHMETIC_ERROR;
}
for (i = 0; i < 3; ++i) {
x[i] /= mold;
}
for (i = 0; i < 2; ++i) {
axs_dir[i] = A[i][0] * ceof[0] + A[i][1] * ceof[1] + A[i][2] * ceof[2];
}
dir = (axs_dir[0] * pca_direction[0] + axs_dir[1] * pca_direction[1] < 0) ? -1 : 1;
for (i = 0; i < 3; ++i) {
ceof[i] = dir * x[i];
}
return;
GET_CEOF_BY_PCA_ARITHMETIC_ERROR:
ceof[0] = 0;
ceof[1] = 1;
ceof[2] = 0;
}
static void correct_axis_ceofficient(float ratio) {
float axis_mean[9];
float pca_direction[2];
float axis_ceofficient[3];
float mold;
int length;
int i;
length = get_optimal_length_by_frequency(g_axis, g_pdr.motionFreq);
get_buffer_mean(&axis_mean[0], &g_axis[0], length);
get_buffer_mean(&axis_mean[3], &g_axis[3], length);
get_buffer_mean(&axis_mean[6], &g_axis[6], length);
get_pca_direction(pca_direction, g_grav, length);
memcpy((void *)axis_ceofficient, (void *)g_pdr.axis_ceofficient, sizeof(g_pdr.axis_ceofficient));
get_ceof_by_pca(axis_ceofficient, axis_mean, pca_direction);
for (i = 0; i < 3; ++i) {
g_pdr.axis_ceofficient[i] = (1 - ratio) * g_pdr.axis_ceofficient[i] + ratio * axis_ceofficient[i];
}
mold = (float)sqrt(g_pdr.axis_ceofficient[0] * g_pdr.axis_ceofficient[0] + g_pdr.axis_ceofficient[1] * g_pdr.axis_ceofficient[1] + g_pdr.axis_ceofficient[2] * g_pdr.axis_ceofficient[2]);
for (i = 0; i < 3; ++i) {
g_pdr.axis_ceofficient[i] /= mold;
}
memset(Pdr.pllaInit, 0, sizeof(Pdr.pllaInit));
}
/**----------------------------------------------------------------------
* Function : pdr_computePCA
* Function : ComputePCA
* Description : 进入RESET_PHASE_1\STABLE阶段之后进行PCA计算
* Date : 2020/7/21 logzhan
*---------------------------------------------------------------------**/
@ -376,24 +79,10 @@ void ComputePCA(AHRS_t *ahrs) {
}
/**----------------------------------------------------------------------
* Function : OutputRawGnssPos
* Description : 输出GPS位置
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void OutputRawGnssPos(GNSS_t* pgnss, LctFs_t* result) {
result->latitude = pgnss->lat;
result->latitude_ns = pgnss->lat_ns;
result->longitudinal = pgnss->lon;
result->longitudinal_ew = pgnss->lon_ew;
result->time = pgnss->timestamps;
}
/**----------------------------------------------------------------------
* Function : pdr_detStatic
* Function : DetUserStatic
* Description : 检测用户是否处于静止状态, 目前主要采用计步器判断和运动频率分析
* 单运动频率计算量太大，可以考虑用加速度判断用户状态。
* Date : 2021/01/28 logzhan
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
int DetUserStatic(PDR_t* g_pdr, GNSS_t* pgnss, unsigned long delSteps) {
@ -414,9 +103,10 @@ int DetUserStatic(PDR_t* g_pdr, GNSS_t* pgnss, unsigned long delSteps) {
* Output : int，车载模式标志位
* Date : 2021/01/28 logzhan
*---------------------------------------------------------------------**/
int DetectCarMode(GNSS_t* pgnss, PDR_t* pdr, unsigned long delSteps, int* time) {
if ((pgnss->vel > 10 && delSteps == 0) ||
(delSteps == 0 && pdr->motionFreq == 0.0 && pgnss->vel > 3 && pgnss->error < 30))
int DetectCarMode(GNSS_t* gnss, PDR_t* pdr, unsigned long delSteps, int* time)
{
if ((gnss->vel > 10 && delSteps == 0) ||
(delSteps == 0 && pdr->motionFreq == 0.0 && gnss->vel > 3 && gnss->error < 30))
{
(*time)++;
}else {
@ -434,14 +124,14 @@ int DetectCarMode(GNSS_t* pgnss, PDR_t* pdr, unsigned long delSteps, int* time)
* 重置PDR到GPS的位置。
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
int DetectPdrToReset(double pdr_angle, int* gpscnt, ulong deltsteps, PDR_t* g_pdr) {
int DetectPdrToReset(double pdr_angle, int* gpsCnt, ulong deltsteps, PDR_t* pdr) {
if ((pdr_angle < 0 && deltsteps > 0) || (deltsteps > 0 && (g_pdr->motionFreq == 0))) {
(*gpscnt)++;
if ((pdr_angle < 0 && deltsteps > 0) || (deltsteps > 0 && (pdr->motionFreq == 0))) {
(*gpsCnt)++;
}else {
*gpscnt = 0;
*gpsCnt = 0;
}
return (*gpscnt > 6);
return (*gpsCnt > 6);
}
/**---------------------------------------------------------------------
@ -449,140 +139,7 @@ int DetectPdrToReset(double pdr_angle, int* gpscnt, ulong deltsteps, PDR_t* g_pd
* Description : 根据新确定的手机携带方式更新状态等
* Date : 2022/10/15 logzhan
*---------------------------------------------------------------------**/
void DetectorUpdate(Detector_t *detector) {
void DetectorUpdate(Detector_t *Detector) {
// 设置PDR的运动类型
g_pdr.motionType = detector->type;
if (DETECTOR_TYPE_SWINGING == g_pdr.motionType && DETECTOR_TYPE_SWINGING != g_motion_type) {
if (!P_TST_BIT(STABLE)) {
P_SET_BIT(CACULATING_AXIS_CEOFFICIENT);
P_CLR_BIT(STABLE);
}
}
else if ((DETECTOR_TYPE_STATIC == g_pdr.motionType || DETECTOR_TYPE_IRREGULAR == g_pdr.motionType) &&
(DETECTOR_TYPE_HANDHELD == g_motion_type || DETECTOR_TYPE_SWINGING == g_motion_type)) {
P_CLR_BIT(CACULATING_AXIS_CEOFFICIENT);
P_CLR_BIT(STABLE);
}
g_motion_type = g_pdr.motionType;
}
/**---------------------------------------------------------------------
* Function : gpsUpdateCb
* Description : gps更新回调函数，目前初步来看主要是计算PCA和GPS方向的区别，
* 分析PCA方向误差作用。
* Date : 2020/02/16 logzhan
*---------------------------------------------------------------------**/
void gpsUpdateCb(GNSS_t* gps)
{
float temp;
float meanA[2];
float meanY[2];
float ctc; // ctc[0][0], ctc[1][1]
float cty[2];
float a[2];
float mold;
float dx;
float dy;
int count;
int index;
/* filter bad point */
double vel_thr;
double error_thr;
if (g_pdr.sceneType) {
vel_thr = 2.5;
error_thr = 6;
}
else {
vel_thr = 1.0;
error_thr = 15;
}
if (fabs(gps->timestamps - g_pdr.gyroTime) > 1000
|| gps->vel < vel_thr \
|| gps->error > error_thr \
|| (DETECTOR_TYPE_SWINGING != g_detector->type && DETECTOR_TYPE_HANDHELD != g_detector->type) \
|| -1 == g_pdr.pca_accuracy \
|| gps->yaw < 0) {
return;
}
/* caculate position */
BufferGetTop((Buffer_t *)&g_posi[0], &temp);
BufferPush((Buffer_t *)&g_posi[0], temp + (float)cos(gps->yaw / 180 * M_PI));
BufferGetTop((Buffer_t *)&g_posi[1], &temp);
BufferPush((Buffer_t *)&g_posi[1], temp + (float)sin(gps->yaw / 180 * M_PI));
BufferGetTop((Buffer_t *)&g_posi[2], &temp);
BufferPush((Buffer_t *)&g_posi[2], temp + g_pdr.pca_direction[0]);
BufferGetTop((Buffer_t *)&g_posi[3], &temp);
BufferPush((Buffer_t *)&g_posi[3], temp + g_pdr.pca_direction[1]);
if ((g_posi[0]._top - g_posi[0]._bottom + g_posi[0].length + 1) % (g_posi[0].length + 1) >= 5) {
count = 0;
meanA[0] = 0;
meanA[1] = 0;
meanY[0] = 0;
meanY[1] = 0;
for (index = g_posi[0]._bottom; index != g_posi[0]._top; index = INC_PTR(&g_posi[0], index)) {
meanA[0] += g_posi[0].data[index];
meanA[1] += g_posi[1].data[index];
meanY[0] += g_posi[2].data[index];
meanY[1] += g_posi[3].data[index];
++count;
}
meanA[0] /= count;
meanA[1] /= count;
meanY[0] /= count;
meanY[1] /= count;
for (index = g_posi[0]._bottom; index != g_posi[0]._top; index = INC_PTR(&g_posi[0], index)) {
g_posi[0].data[index] -= meanA[0];
g_posi[1].data[index] -= meanA[1];
g_posi[2].data[index] -= meanY[0];
g_posi[3].data[index] -= meanY[1];
}
ctc = 0;
cty[0] = 0;
cty[1] = 0;
for (index = g_posi[0]._bottom; index != g_posi[0]._top; index = INC_PTR(&g_posi[0], index)) {
ctc += g_posi[0].data[index] * g_posi[0].data[index] + g_posi[1].data[index] * g_posi[1].data[index];
cty[0] += g_posi[0].data[index] * g_posi[2].data[index] + g_posi[1].data[index] * g_posi[3].data[index];
cty[1] += -g_posi[1].data[index] * g_posi[2].data[index] + g_posi[0].data[index] * g_posi[3].data[index];
}
if (0 != ctc) {
/* (c^Tc)^{-1}c^Ty */
a[0] = cty[0] / ctc;
a[1] = cty[1] / ctc;
}
else {
goto GPS_UPDATE_ARITHMETIC_ERROR;
}
if (0 == (mold = (float)GET_MOL(a))) {
goto GPS_UPDATE_ARITHMETIC_ERROR;
}
g_pdr.bias_direction[0] = a[0] / mold;
g_pdr.bias_direction[1] = a[1] / mold;
g_pdr.bias_accuracy = 0;
for (index = g_posi[0]._bottom; index != g_posi[0]._top; index = INC_PTR(&g_posi[0], index)) {
dx = g_posi[2].data[index] - a[0] * g_posi[0].data[index] + a[1] * g_posi[1].data[index];
dy = g_posi[3].data[index] - a[1] * g_posi[0].data[index] - a[0] * g_posi[1].data[index];
g_pdr.bias_accuracy += dx * dx + dy * dy;
}
g_pdr.bias_accuracy = (float)sqrt(g_pdr.bias_accuracy / count);
P_SET_BIT(BIAS_STABLE);
return;
}
GPS_UPDATE_ARITHMETIC_ERROR:
g_pdr.bias_direction[0] = 1;
g_pdr.bias_direction[1] = 0;
g_pdr.bias_accuracy = -1;
P_CLR_BIT(BIAS_STABLE);
Pdr.MotionType = Detector->type;
}