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

优化kalmam滤波器的矩阵初始化逻辑

PDRHistoryBranch
詹力 2022-10-09 23:49:35 +08:00
parent 474931c228
commit b4aeec5580
2 changed files with 6 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -53,24 +53,6 @@ void EKF_Init(void)
hold_type = 0; 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 * 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 (*pvec1)[N] = (double(*)[N])&(EkfMatBuf[1][0][0]);
double (*pvec2)[N] = (double(*)[N])&(EkfMatBuf[2][0][0]); double (*pvec2)[N] = (double(*)[N])&(EkfMatBuf[2][0][0]);
clear_buf((double *)Phi, BUF_DMT_1); memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
clear_buf((double *)pvec1, BUF_DMT_1);
clear_buf((double *)pvec2, BUF_DMT_1);
double deltaHeading = g_pdr->heading - g_pdr->lastHeading; double deltaHeading = g_pdr->heading - g_pdr->lastHeading;
double angle = g_pdr->heading; double angle = g_pdr->heading;
//printf("deltaHeading = %f\n", R2D(deltaHeading)); //printf("deltaHeading = %f\n", R2D(deltaHeading));
//if (fabs(deltaHeading) < D2R(10) && g_pdr->lastHeading != 0.0f) { //if (fabs(deltaHeading) < D2R(10) && g_pdr->lastHeading != 0.0f) {
// angle = deltaHeading + kf->p_xk[3]; // angle = deltaHeading + kf->p_xk[3];
//}else { //}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(*pvec4)[N] = (double(*)[N])&(EkfMatBuf[5][0][0]);
double(*pv3)[N] = (double(*)[N])&(EkfMatBuf[6][0][0]); double(*pv3)[N] = (double(*)[N])&(EkfMatBuf[6][0][0]);
// 初始化变量 memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
clear_buf((double*)H, BUF_DMT_2); memset(EkfVecBuf, 0, sizeof(double) * 5 * N);
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);
// Nx1向量定义 // Nx1向量定义
double* z = &(EkfVecBuf[0][0]); 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(*pv1) = &(EkfVecBuf[3][0]);
double(*pv2) = &(EkfVecBuf[4][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++) { for (char i = 0; i < 4; i++) {
H[i][i] = 1; 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++) { for (uchar i = 0; i < N; i++) {
kf->xk[i] = kf->p_xk[i]; kf->xk[i] = kf->p_xk[i];
} }
// 清除卡尔曼Q矩阵和R矩阵
for (int i = 0; i < N; i++) { for (int i = 0; i < N; i++) {
for (int l = 0; l < N; l++) { for (int l = 0; l < N; l++) {
kf->q[i][l] = 0.0; kf->q[i][l] = 0.0;
kf->r[i][l] = 0.0; kf->r[i][l] = 0.0;
} }
} }
//printf("重置PDR滤波器，输出GPS位置\n");
// 输出GPS位置结果 // 输出GPS位置结果
OutputOriginGnssPos(pgnss, result); OutputOriginGnssPos(pgnss, result);
g_pdr->NoGnssCount = 0; g_pdr->NoGnssCount = 0;

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

@ -80,7 +80,7 @@ extern "C" double refGpsYaw;
int main(int argc, char* argv[]) 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 logDate = "shenzhen";
string logLabel = "data\\"; string logLabel = "data\\";
string fileHead; string fileHead;
@ -129,7 +129,7 @@ int main(int argc, char* argv[])
// 关闭PDR算法，释放资源 // 关闭PDR算法，释放资源
Algorithm_DeInit(); Algorithm_DeInit();
// 写结果kml // 写结果kml
KmlWrite(kmlPath, fileHead, string("_PDR_ZL_2.1b")); KmlWrite(kmlPath, fileHead, string("_PDR_ZL_2.1c"));
// 关闭文件 // 关闭文件
fclose(fileFp); fclose(fileFp);
} }