From e9eb044b0b9a4590250badac59b1e047386e3ebb Mon Sep 17 00:00:00 2001
From: logzhan <719901725@qq.com>
Date: Wed, 19 Oct 2022 13:11:33 +0800
Subject: [PATCH] =?UTF-8?q?=E5=B0=86=E8=A7=92=E5=BA=A6=E9=A2=84=E6=B5=8B?=
 =?UTF-8?q?=E4=BB=8EKalman=E8=BF=81=E7=A7=BB=E5=88=B0Location?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 1.Software/PDR 1.05/src/Kalman.c   | 39 ++++++------------------------
 1.Software/PDR 1.05/src/Location.c | 23 ++++++++++++------
 1.Software/PDR 1.05/src/PDRBase.c  |  7 ++++++
 3 files changed, 31 insertions(+), 38 deletions(-)

diff --git a/1.Software/PDR 1.05/src/Kalman.c b/1.Software/PDR 1.05/src/Kalman.c
index 37ccb18..b8d5263 100644
--- a/1.Software/PDR 1.05/src/Kalman.c	
+++ b/1.Software/PDR 1.05/src/Kalman.c	
@@ -38,42 +38,19 @@ void EKF_Init(void)
 *---------------------------------------------------------------------**/
 void EKFStatePredict(EKFPara_t *ekf, double stepLen, PDR_t* pdr, int step) {
 
-	double (*Phi)[N]   = (double(*)[N])&(EkfMatBuf[0][0][0]);
-	double (*pvec1)[N] = (double(*)[N])&(EkfMatBuf[1][0][0]);
-	double (*pvec2)[N] = (double(*)[N])&(EkfMatBuf[2][0][0]);
-
-	memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
-
-	double deltaHeading = pdr->heading - pdr->lastHeading;
-	double angle        = pdr->heading;
-
-	//printf("deltaHeading = %f\n", R2D(deltaHeading));
-	//if (fabs(deltaHeading) < D2R(10) && g_pdr->lastHeading != 0.0f) {
-	//	angle = deltaHeading + kf->pXk[3];
-	//}else {
-	//	angle = g_pdr->heading;
-	//}
-
-	if (pdr->MotionType != PDR_MOTION_TYPE_HANDHELD) {
-		deltaHeading = 0;
-	}
-
-	angle = deltaHeading + ekf->pXk[3];
-
-	if (fabs(deltaHeading) > 5.5) {
-		if (deltaHeading < 0) {
-			deltaHeading = deltaHeading + D2R(360);
-		}else {
-			deltaHeading = deltaHeading - D2R(360);
-		}
-		deltaHeading = 0;
-		angle = deltaHeading + ekf->pXk[3];
-	}
+	double angle = pdr->deltaHeading + ekf->pXk[3];
 
 	if(step == 0){
 		ekf->pXk[3] = angle;
+		return;
 	}
 
+	double(*Phi)[N] = (double(*)[N]) & (EkfMatBuf[0][0][0]);
+	double(*pvec1)[N] = (double(*)[N]) & (EkfMatBuf[1][0][0]);
+	double(*pvec2)[N] = (double(*)[N]) & (EkfMatBuf[2][0][0]);
+
+	memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
+
     // xk+1 = xk + step * cos(angle)   PDR位置更新方程
 	// yk+1 = yk + step * sin(angle)   PDR位置更新方程
 	// sk+1 = sk                       步长和上一次相同
diff --git a/1.Software/PDR 1.05/src/Location.c b/1.Software/PDR 1.05/src/Location.c
index 851b0de..517e2e8 100644
--- a/1.Software/PDR 1.05/src/Location.c	
+++ b/1.Software/PDR 1.05/src/Location.c	
@@ -158,15 +158,24 @@ int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) {
 
     //pdr.heading = CalPredHeading(...);
 
-    double imuYaw = - Ahrs.Yaw;
-    if (imuYaw < 0) {
-        imuYaw += 360;
+    //if (pdr.MotionType == PDR_MOTION_TYPE_HANDHELD) 
+    {
+        double imuHeading = -Ahrs.Yaw;
+        if (imuHeading < 0) {
+            imuHeading += 360;
+        }
+
+        pdr.heading = D2R(imuHeading);
+        if (fabs(pdr.insHeadingOffset) > 0.000001) {
+            imuHeading = imuHeading - pdr.insHeadingOffset * (180 / 3.14159265);
+            pdr.heading = imuHeading / (180 / 3.14159265);
+        }
+        pdr.deltaHeading = pdr.heading - pdr.lastHeading;
     }
 
-    pdr.heading = imuYaw / (180 / 3.14159265);
-    if (fabs(pdr.insHeadingOffset) > 0.000001) {
-        imuYaw = imuYaw - pdr.insHeadingOffset * (180 / 3.14159265);
-        pdr.heading = imuYaw / (180 / 3.14159265);
+    // 角度增量约束(100Hz * 5.5° = 550°/sec)
+    if (pdr.deltaHeading > D2R(5.5)) {
+        pdr.deltaHeading = 0.0f;
     }
 
     // PDR 惯性位置预测
diff --git a/1.Software/PDR 1.05/src/PDRBase.c b/1.Software/PDR 1.05/src/PDRBase.c
index e2929b6..7b1c107 100644
--- a/1.Software/PDR 1.05/src/PDRBase.c	
+++ b/1.Software/PDR 1.05/src/PDRBase.c	
@@ -255,4 +255,11 @@ void InsLocationPredict(PDR_t* pdr, StepPredict* stepPredict, IMU_t* imu,
 			if (PreStep > 0)pdr->fusionPdrFlg = PDR_TRUE;
 		}
 	}
+
+	// 如果计步器可以保持较高的实时性，就可以采用下面的方法直接计算
+	//if (pdr->heading > 0) {
+	//	deltaStep = steps - lastSteps;
+	//	EKFStatePredict(kf, stepLength, pdr, deltaStep);
+	//	pdr->fusionPdrFlg = PDR_TRUE;
+	//}
 }
\ No newline at end of file