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