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

将角度预测从Kalman迁移到Location

PDRHistoryBranch
詹力 2022-10-19 13:11:33 +08:00
parent 3b19ed22e2
commit e9eb044b0b
3 changed files with 31 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
1.Software/PDR 1.05/src/Kalman.c
View File

@ -38,42 +38,19 @@ void EKF_Init(void)
*---------------------------------------------------------------------**/ *---------------------------------------------------------------------**/
void EKFStatePredict(EKFPara_t *ekf, double stepLen, PDR_t* pdr, int step) { void EKFStatePredict(EKFPara_t *ekf, double stepLen, PDR_t* pdr, int step) {
double (*Phi)[N] = (double(*)[N])&(EkfMatBuf[0][0][0]); double angle = pdr->deltaHeading + ekf->pXk[3];
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];
}
if(step == 0){ if(step == 0){
ekf->pXk[3] = angle; 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位置更新方程 // xk+1 = xk + step * cos(angle) PDR位置更新方程
// yk+1 = yk + step * sin(angle) PDR位置更新方程 // yk+1 = yk + step * sin(angle) PDR位置更新方程
// sk+1 = sk 步长和上一次相同 // sk+1 = sk 步长和上一次相同

23
1.Software/PDR 1.05/src/Location.c
View File

@ -158,15 +158,24 @@ int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) {
//pdr.heading = CalPredHeading(...); //pdr.heading = CalPredHeading(...);
double imuYaw = - Ahrs.Yaw; //if (pdr.MotionType == PDR_MOTION_TYPE_HANDHELD)
if (imuYaw < 0) { {
imuYaw += 360; 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); // ½Ç¶ÈÔöÁ¿Ô¼Êø(100Hz * 5.5¡ã = 550¡ã/sec)
if (fabs(pdr.insHeadingOffset) > 0.000001) { if (pdr.deltaHeading > D2R(5.5)) {
imuYaw = imuYaw - pdr.insHeadingOffset * (180 / 3.14159265); pdr.deltaHeading = 0.0f;
pdr.heading = imuYaw / (180 / 3.14159265);
} }
// PDR 惯性位置预测 // PDR 惯性位置预测

7
1.Software/PDR 1.05/src/PDRBase.c
View File

@ -255,4 +255,11 @@ void InsLocationPredict(PDR_t* pdr, StepPredict* stepPredict, IMU_t* imu,
if (PreStep > 0)pdr->fusionPdrFlg = PDR_TRUE; if (PreStep > 0)pdr->fusionPdrFlg = PDR_TRUE;
} }
} }
// 如果计步器可以保持较高的实时性，就可以采用下面的方法直接计算
//if (pdr->heading > 0) {
// deltaStep = steps - lastSteps;
// EKFStatePredict(kf, stepLength, pdr, deltaStep);
// pdr->fusionPdrFlg = PDR_TRUE;
//}
} }