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

优化卡尔曼滤波器的命名

PDRHistoryBranch
詹力 2022-10-10 23:41:17 +08:00
parent b4aeec5580
commit 050fb4b3d1
3 changed files with 80 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
1.Software/PDR 1.04/include/pdr_base.h
View File

@ -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;

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

@ -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);

8
1.Software/PDR 1.04/src/Location.c
View File

@ -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);