From 050fb4b3d1b1628ced2442fbeab170bc8dbc22bd Mon Sep 17 00:00:00 2001 From: logzhan <719901725@qq.com> Date: Mon, 10 Oct 2022 23:41:17 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=E5=8D=A1=E5=B0=94=E6=9B=BC?= =?UTF-8?q?=E6=BB=A4=E6=B3=A2=E5=99=A8=E7=9A=84=E5=91=BD=E5=90=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- 1.Software/PDR 1.04/include/pdr_base.h | 22 ++--- 1.Software/PDR 1.04/src/Kalman.c | 130 ++++++++++++------------- 1.Software/PDR 1.04/src/Location.c | 8 +- 3 files changed, 80 insertions(+), 80 deletions(-) diff --git a/1.Software/PDR 1.04/include/pdr_base.h b/1.Software/PDR 1.04/include/pdr_base.h index 0ff26a0..0125d36 100644 --- a/1.Software/PDR 1.04/include/pdr_base.h +++ b/1.Software/PDR 1.04/include/pdr_base.h @@ -102,19 +102,19 @@ typedef enum { typedef struct { - double xk[N]; // 状态变量Xk xk[0]: 北向x xk[1]：东向y xk[2]：步长 xk[3] ：航向角 - double p_xk[N]; // 最佳预测变量Xk_predict xk[0]: 北向x xk[1]：东向y xk[2]：步长 xk[3] ：航向角 - double zk[N]; - double p_pk[N][N]; - double pk[N][N]; - double phi[N][N]; + double Xk[N]; // 状态变量Xk xk[0]: 北向x xk[1]：东向y xk[2]：步长 xk[3] ：航向角 + double pXk[N]; // 最佳预测变量Xk_predict xk[0]: 北向x xk[1]：东向y xk[2]：步长 xk[3] ：航向角 + double Zk[N]; + double pPk[N][N]; + double Pk[N][N]; + double Phi[N][N]; double hk[N][N]; - double q[N][N]; // 卡尔曼滤波的Q矩阵(过程噪声) - double r[N][N]; // 卡尔曼滤波R矩阵(观测噪声) + double Q[N][N]; // 卡尔曼滤波的Q矩阵(过程噪声) + double R[N][N]; // 卡尔曼滤波R矩阵(观测噪声) double Kk[N][N]; - double lambda; - double plat; - double plon; + double Lambda; + double pLat; + double pLon; double initHeading; }EKFPara_t; diff --git a/1.Software/PDR 1.04/src/Kalman.c b/1.Software/PDR 1.04/src/Kalman.c index a3726d4..fbfa374 100644 --- a/1.Software/PDR 1.04/src/Kalman.c +++ b/1.Software/PDR 1.04/src/Kalman.c @@ -72,7 +72,7 @@ void EKFStatePredict(EKFPara_t *kf, double stepLen, PDR_t* g_pdr, int step) { //printf("deltaHeading = %f\n", R2D(deltaHeading)); //if (fabs(deltaHeading) < D2R(10) && g_pdr->lastHeading != 0.0f) { - // angle = deltaHeading + kf->p_xk[3]; + // angle = deltaHeading + kf->pXk[3]; //}else { // angle = g_pdr->heading; //} @@ -81,7 +81,7 @@ void EKFStatePredict(EKFPara_t *kf, double stepLen, PDR_t* g_pdr, int step) { deltaHeading = 0; } - angle = deltaHeading + kf->p_xk[3]; + angle = deltaHeading + kf->pXk[3]; if (fabs(deltaHeading) > 5.5) { @@ -93,11 +93,11 @@ void EKFStatePredict(EKFPara_t *kf, double stepLen, PDR_t* g_pdr, int step) { deltaHeading = deltaHeading - D2R(360); } deltaHeading = 0; - angle = deltaHeading + kf->p_xk[3]; + angle = deltaHeading + kf->pXk[3]; } if(step == 0){ - kf->p_xk[3] = angle; + kf->pXk[3] = angle; } // xk+1 = xk + step * cos(angle) PDR位置更新方程 @@ -116,21 +116,21 @@ void EKFStatePredict(EKFPara_t *kf, double stepLen, PDR_t* g_pdr, int step) { // phi[3][3]是为让kf的预测每次都是最新更新值, 如果phi[3][3]每次都是1，可能在惯性 // 导航更新过程中，偏航角的预测是固定的，实时性没有那么好，下面if判断可以在一定程度 // 让预测的值更新，但是效果暂时没有感觉出明显的好 - if (fabs(kf->p_xk[3]) > 0.000001 && - fabs(angle / kf->p_xk[3]) < 3) + if (fabs(kf->pXk[3]) > 0.000001 && + fabs(angle / kf->pXk[3]) < 3) { - Phi[3][3] = angle / kf->p_xk[3]; + Phi[3][3] = angle / kf->pXk[3]; } // 卡尔曼状态更新方程 - // p_xk = phi * p_xk - VecMatMultiply(kf->p_xk, Phi, kf->p_xk); + // pXk = phi * pXk + VecMatMultiply(kf->pXk, Phi, kf->pXk); // 卡尔曼更新协方差矩阵 : Pk = FPFt + Q - MatrixMultiply(Phi, kf->p_pk, pvec1); // F*P + MatrixMultiply(Phi, kf->pPk, pvec1); // F*P MatrixTrans(Phi, pvec2); // Ft MatrixMultiply(pvec1, pvec2, pvec1); // F*P*Ft - MatrixAdd(pvec1, kf->q, kf->p_pk); // F*P*Ft + Q + MatrixAdd(pvec1, kf->Q, kf->pPk); // F*P*Ft + Q } } @@ -185,16 +185,16 @@ void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss, classifer *sys, PDR_t *g_pdr, // //} // z[2] = pgnss->vel * 0.514 / (g_pdr->deltaStepsPerGps); //}else { - // z[2] = kf->p_xk[2]; + // z[2] = kf->pXk[2]; //} z[3] = pgnss->yaw * PI / 180; // 观测的航向角为GPS的航向角 // 计算 K = K = PHt / (R + H*P*H^T) MatrixTrans(H, Ht); // 计算Ht - MatrixMultiply(H, kf->p_pk, pvec1); // 计算HP + MatrixMultiply(H, kf->pPk, pvec1); // 计算HP MatrixMultiply(pvec1, Ht, pvec3); // pvec3 = H*P*H^T - MatrixAdd(pvec3, kf->r, pvec4); // pvec4 = R + H*P*H^T + MatrixAdd(pvec3, kf->R, pvec4); // pvec4 = R + H*P*H^T MatrixInverse(pvec4, pvec1); // 计算 1 / (R + H*P*H^T) for (char i = 0; i < N; i++) { @@ -202,11 +202,11 @@ void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss, classifer *sys, PDR_t *g_pdr, pv3[i][j] = pvec3[i][j]; } } - MatrixMultiply(kf->p_pk, Ht, pvec3); // 计算PHt + MatrixMultiply(kf->pPk, Ht, pvec3); // 计算PHt MatrixMultiply(pvec3, pvec1, kf->Kk); // 计算增益K = PHt / (R + H*P*H^T) - VecMatMultiply(kf->p_xk, H, pvec5); // pvec5 = Hx' + VecMatMultiply(kf->pXk, H, pvec5); // pvec5 = Hx' VectorSub(z, pvec5, pvec6); // pvec6 = Z - Hx' modAngle(&pvec6[3], -PI, PI); for (char i = 0; i < N; i++) { @@ -215,7 +215,7 @@ void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss, classifer *sys, PDR_t *g_pdr, VecMatMultiply(pvec6, kf->Kk, pvec5); // pvec5 = K*( z - Hx') // Get the optimally updated state estimation - VectorAdd(kf->p_xk, pvec5, kf->xk); + VectorAdd(kf->pXk, pvec5, kf->Xk); // ---- prepare the matrix for updated estimate covariance ------ // pvec1 = K*H @@ -223,20 +223,20 @@ void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss, classifer *sys, PDR_t *g_pdr, // pvec2 = (I - K*H) MatrixSub(I, pvec1, Ht); // pvec3 = (I - K*H)*P - MatrixMultiply(Ht, kf->p_pk, pvec3); + MatrixMultiply(Ht, kf->pPk, pvec3); // pvec4 = (I- K*H)^T MatrixTrans(Ht, pvec4); // pvec1 = (I - K*H)*P*(I- K*H)^T MatrixMultiply(pvec3, pvec4, pvec1); // pvec2 = K*R - MatrixMultiply(kf->Kk, kf->r, Ht); + MatrixMultiply(kf->Kk, kf->R, Ht); // pvec3 = K^T MatrixTrans(kf->Kk, pvec3); // pvec4 = K*R*K^T MatrixMultiply(Ht, pvec3, pvec4); // Get the updated estimate covariance: P' = (I - K*H)*P*(I- K*H)^T + K*R*K^T - MatrixAdd(pvec1, pvec4, kf->pk); + MatrixAdd(pvec1, pvec4, kf->Pk); // pv2 = (z - Hx')*( H*P*H^T ) VecMatMultiply(pv1, pv3, pv2); @@ -249,16 +249,16 @@ void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss, classifer *sys, PDR_t *g_pdr, lambda += pv2[i] * pv1[i]; } - kf->lambda = lambda; - kf->plat = pgnss->lat; - kf->plon = pgnss->lon; + kf->Lambda = lambda; + kf->pLat = pgnss->lat; + kf->pLon = pgnss->lon; if (lambda >= 200)return; for (char i = 0; i < N; i++) { - kf->xk[i] = kf->p_xk[i]; + kf->Xk[i] = kf->pXk[i]; for (int j = 0; j < N; j++) { - kf->pk[i][j] = kf->p_pk[i][j]; + kf->Pk[i][j] = kf->pPk[i][j]; } } @@ -526,20 +526,20 @@ int ResetOutputResult(GNSS_t *pgnss, PDR_t* g_pdr, EKFPara_t *kf, lct_fs *result g_pdr->x0 = ned[0]; g_pdr->y0 = ned[1]; - kf->p_xk[0] = ned[0]; - kf->p_xk[1] = ned[1]; - kf->p_xk[2] = stepLength; + kf->pXk[0] = ned[0]; + kf->pXk[1] = ned[1]; + kf->pXk[2] = stepLength; // 重置输出GPS时，GPS的航向角不一定准 - kf->p_xk[3] = pgnss->yaw*RADIAN_PER_DEG; + kf->pXk[3] = pgnss->yaw*RADIAN_PER_DEG; for (uchar i = 0; i < N; i++) { - kf->xk[i] = kf->p_xk[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; + kf->Q[i][l] = 0.0; + kf->R[i][l] = 0.0; } } // 输出GPS位置结果 @@ -567,20 +567,20 @@ int InitProc(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr,lct_fs *result) g_pdr->y0 = ned[1]; // 初始化卡尔曼滤波器的观测量 - kf->p_xk[0] = ned[0]; // 状态量1: 北向x - kf->p_xk[1] = ned[1]; // 状态量2：东向y - kf->p_xk[2] = stepLen; // 状态量3：步长 - kf->p_xk[3] = pgnss->yaw * RADIAN_PER_DEG; // 状态量4：航向角 + 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->p_xk[i]; + kf->Xk[i] = kf->pXk[i]; } - kf->initHeading = kf->p_xk[3]; + 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; + kf->Q[i][l] = 0.0; + kf->R[i][l] = 0.0; } } OutputOriginGnssPos(pgnss, result); @@ -608,41 +608,41 @@ void EKFCalQRMatrix(lct_nmea *nmea, PDR_t *g_pdr, classifer *sys, 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步长噪声 - kf->q[3][3] = 100; // PDR方向噪声 + kf->Q[0][0] = 25; // PDR预测位置噪声 + kf->Q[1][1] = 25; // PDR预测位置噪声 + kf->Q[2][2] = 1; // PDR步长噪声 + kf->Q[3][3] = 100; // PDR方向噪声 - kf->r[0][0] = 50; // GPS观测位置噪声 - kf->r[1][1] = 50; // GPS观测位置噪声 - kf->r[2][2] = 0.1; // GPS步长噪声 - kf->r[3][3] = 50; // GPS方向噪声 + kf->R[0][0] = 50; // GPS观测位置噪声 + kf->R[1][1] = 50; // GPS观测位置噪声 + kf->R[2][2] = 0.1; // GPS步长噪声 + kf->R[3][3] = 50; // GPS方向噪声 return; } - kf->q[0][0] = 0.1; // PDR预测位置噪声 - kf->q[1][1] = 0.1; // PDR预测位置噪声 - kf->q[2][2] = 1; // PDR步长噪声 - kf->q[3][3] = 1; // PDR方向噪声 + kf->Q[0][0] = 0.1; // PDR预测位置噪声 + kf->Q[1][1] = 0.1; // PDR预测位置噪声 + kf->Q[2][2] = 1; // PDR步长噪声 + kf->Q[3][3] = 1; // PDR方向噪声 - kf->r[0][0] = 100; // GPS预测位置噪声 - kf->r[1][1] = 100; // GPS预测位置噪声 - kf->r[2][2] = 0.1; // GPS步长噪声 - kf->r[3][3] = 1000; // GPS方向噪声 + kf->R[0][0] = 100; // GPS预测位置噪声 + kf->R[1][1] = 100; // GPS预测位置噪声 + kf->R[2][2] = 0.1; // GPS步长噪声 + kf->R[3][3] = 1000; // GPS方向噪声 if (nmea->accuracy.err > 0 && nmea->accuracy.err < 3.80 && nmea->gga.hdop <= 0.4) { - kf->r[0][0] = 50; // GPS观测位置噪声 - kf->r[1][1] = 50; // GPS观测位置噪声 - kf->r[2][2] = 0.1; // GPS步长噪声 - kf->r[3][3] = 50; // GPS方向噪声 + kf->R[0][0] = 50; // GPS观测位置噪声 + kf->R[1][1] = 50; // GPS观测位置噪声 + kf->R[2][2] = 0.1; // GPS步长噪声 + kf->R[3][3] = 50; // GPS方向噪声 } if (fabs(R2D((g_pdr->gpsHeading - g_pdr->lastGpsHeading))) > 50 || g_pdr->gpsSpeed < 0.7 || g_pdr->gpsHeading == -1) { - kf->r[3][3] = 10000; + kf->R[3][3] = 10000; } } /**---------------------------------------------------------------------- @@ -672,13 +672,13 @@ void GnssInsLocFusion(GNSS_t *pgnss, PDR_t *g_pdr, classifer *sys, double yaw_bi for (uchar i = 0; i < N; i++) { for (uchar l = 0; l < N; l++) { - kf->p_pk[i][l] = kf->pk[i][l]; + kf->pPk[i][l] = kf->Pk[i][l]; } - kf->p_xk[i] = kf->xk[i]; + kf->pXk[i] = kf->Xk[i]; } - ned[0] = kf -> p_xk[0] - g_pdr->x0; - ned[1] = kf -> p_xk[1] - g_pdr->y0; + ned[0] = kf -> pXk[0] - g_pdr->x0; + ned[1] = kf -> pXk[1] - g_pdr->y0; ned[2] = 0; NED2WGS84(g_pdr->pllaInit, ned, plla); diff --git a/1.Software/PDR 1.04/src/Location.c b/1.Software/PDR 1.04/src/Location.c index 69bb80b..f91aec0 100644 --- a/1.Software/PDR 1.04/src/Location.c +++ b/1.Software/PDR 1.04/src/Location.c @@ -180,7 +180,7 @@ int GnssInsFusionLocation(lct_nmea *nmea_data, EKFPara_t *kf, lct_fs *result) // 如果在win32平台下仿真调试,则添加下面代码 #ifdef _WIN32 - result->pdrHeading = kf->p_xk[3]; + result->pdrHeading = kf->pXk[3]; result->gpsHeading = pdr->gpsHeading; result->hdop = pgnss.HDOP; result->accuracy = nmea_data->accuracy.err; @@ -217,7 +217,7 @@ int GnssInsFusionLocation(lct_nmea *nmea_data, EKFPara_t *kf, lct_fs *result) //}else if (pdr->trackHeading != INVAILD_GPS_YAW) { // pgnss.yaw = (float)(R2D(pdr->trackHeading)); //} - pgnss.yaw = (float)kf->p_xk[3]; + pgnss.yaw = (float)kf->pXk[3]; } if (pgnss.yaw != INVAILD_GPS_YAW) { GnssInsLocFusion(&pgnss, pdr, &sys, yaw_bias, kf, result); @@ -251,8 +251,8 @@ int GnssInsFusionLocation(lct_nmea *nmea_data, EKFPara_t *kf, lct_fs *result) void NoGnssInfoPredict(EKFPara_t* kf, lct_fs* result, PDR_t* p_pdr) { double ned[3] = {0}, lla[3] = {0}; - ned[0] = kf->p_xk[0] - p_pdr->x0; - ned[1] = kf->p_xk[1] - p_pdr->y0; + ned[0] = kf->pXk[0] - p_pdr->x0; + ned[1] = kf->pXk[1] - p_pdr->y0; ned[2] = 0; NED2WGS84(p_pdr->pllaInit, ned, lla);