From b4aeec55800f8dc249d86cb61290077bf22b2b2c Mon Sep 17 00:00:00 2001
From: logzhan <719901725@qq.com>
Date: Sun, 9 Oct 2022 23:49:35 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96kalmam=E6=BB=A4=E6=B3=A2?=
 =?UTF-8?q?=E5=99=A8=E7=9A=84=E7=9F=A9=E9=98=B5=E5=88=9D=E5=A7=8B=E5=8C=96?=
 =?UTF-8?q?=E9=80=BB=E8=BE=91?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 1.Software/PDR 1.04/src/Kalman.c | 43 +++-----------------------------
 1.Software/PDR 1.04/src/Main.cpp |  4 +--
 2 files changed, 6 insertions(+), 41 deletions(-)

diff --git a/1.Software/PDR 1.04/src/Kalman.c b/1.Software/PDR 1.04/src/Kalman.c
index 3bea9e2..a3726d4 100644
--- a/1.Software/PDR 1.04/src/Kalman.c	
+++ b/1.Software/PDR 1.04/src/Kalman.c	
@@ -53,24 +53,6 @@ void EKF_Init(void)
 	hold_type = 0;
 }
 
-int clear_buf(double *buf_p, int dmt)
-{
-	int bufLen = 0;
-	double *buf_pdr = (double *)buf_p;
-
-	if (NULL == buf_p)return -1;
-
-	if (!(2 == dmt || 1 == dmt)){
-		return -1;
-	}
-	else if (2 == dmt)bufLen = N * N;
-	else if (1 == dmt)bufLen = N;
-
-	for (int i = 0; i < bufLen; i++){
-		buf_pdr[i] = 0.0f;
-	}
-	return 0;
-}
 
 /**----------------------------------------------------------------------
 * Function    : pdr_ekfStatePredict
@@ -83,15 +65,12 @@ void EKFStatePredict(EKFPara_t *kf, double stepLen, PDR_t* g_pdr, int step) {
 	double (*pvec1)[N] = (double(*)[N])&(EkfMatBuf[1][0][0]);
 	double (*pvec2)[N] = (double(*)[N])&(EkfMatBuf[2][0][0]);
 
-	clear_buf((double *)Phi, BUF_DMT_1);
-	clear_buf((double *)pvec1, BUF_DMT_1);
-	clear_buf((double *)pvec2, BUF_DMT_1);
+	memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
 
 	double deltaHeading = g_pdr->heading - g_pdr->lastHeading;
 	double angle        = g_pdr->heading;
 
 	//printf("deltaHeading = %f\n", R2D(deltaHeading));
-
 	//if (fabs(deltaHeading) < D2R(10) && g_pdr->lastHeading != 0.0f) {
 	//	angle = deltaHeading + kf->p_xk[3];
 	//}else {
@@ -174,14 +153,8 @@ void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss,  classifer *sys, PDR_t *g_pdr,
 	double(*pvec4)[N] = (double(*)[N])&(EkfMatBuf[5][0][0]);
 	double(*pv3)[N]   = (double(*)[N])&(EkfMatBuf[6][0][0]);
 
-	// 初始化变量
-	clear_buf((double*)H,     BUF_DMT_2);
-	clear_buf((double*)I,     BUF_DMT_2);
-	clear_buf((double*)pvec1, BUF_DMT_2);
-	clear_buf((double*)Ht,    BUF_DMT_2);
-	clear_buf((double*)pvec3, BUF_DMT_2);
-	clear_buf((double*)pvec4, BUF_DMT_2);
-	clear_buf((double*)pv3,   BUF_DMT_2);
+	memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
+	memset(EkfVecBuf, 0, sizeof(double) * 5 * N);
 
 	// Nx1向量定义
 	double* z         = &(EkfVecBuf[0][0]);
@@ -190,13 +163,6 @@ void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss,  classifer *sys, PDR_t *g_pdr,
 	double(*pv1)      = &(EkfVecBuf[3][0]);
 	double(*pv2)      = &(EkfVecBuf[4][0]);
 	
-	// 初始化变量
-	clear_buf((double *)z,     BUF_DMT_1);
-	clear_buf((double *)pvec5, BUF_DMT_1);
-	clear_buf((double *)pvec6, BUF_DMT_1);
-	clear_buf((double *)pv1,   BUF_DMT_1);
-	clear_buf((double *)pv2,   BUF_DMT_1);
-
 	// 创建单位矩阵
 	for (char i = 0; i < 4; i++) {
 		H[i][i] = 1;
@@ -569,14 +535,13 @@ int ResetOutputResult(GNSS_t *pgnss, PDR_t* g_pdr, EKFPara_t *kf, lct_fs *result
 	for (uchar i = 0; i < N; i++) {
 		kf->xk[i] = kf->p_xk[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;
 		}
 	}
-	//printf("重置PDR滤波器，输出GPS位置\n");
 	// 输出GPS位置结果
 	OutputOriginGnssPos(pgnss, result);
 	g_pdr->NoGnssCount = 0;
diff --git a/1.Software/PDR 1.04/src/Main.cpp b/1.Software/PDR 1.04/src/Main.cpp
index 067bcc6..45ed90c 100644
--- a/1.Software/PDR 1.04/src/Main.cpp	
+++ b/1.Software/PDR 1.04/src/Main.cpp	
@@ -80,7 +80,7 @@ extern "C" double refGpsYaw;
 int main(int argc, char* argv[])
 {
 
-	string logPath  = "E:\\SoftwareProject\\GnssIns\\GnssIns-PDR-Private\\1.Software\\PDR 1.02\\Debug\\";
+	string logPath  = "E:\\SoftwareProject\\GnssIns\\GnssIns-PDR-Private\\1.Software\\PDR 1.04\\Debug\\";
 	string logDate  = "shenzhen";
 	string logLabel = "data\\";
 	string fileHead;
@@ -129,7 +129,7 @@ int main(int argc, char* argv[])
 		// 关闭PDR算法，释放资源
 		Algorithm_DeInit();
 		// 写结果kml
-		KmlWrite(kmlPath, fileHead, string("_PDR_ZL_2.1b"));
+		KmlWrite(kmlPath, fileHead, string("_PDR_ZL_2.1c"));
 		// 关闭文件
 		fclose(fileFp);
 	}