From 3b19ed22e28cc45eeced030df4430d5b0590520c Mon Sep 17 00:00:00 2001 From: logzhan <719901725@qq.com> Date: Tue, 18 Oct 2022 13:36:05 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A4=A7=E5=B9=85=E5=BA=A6=E7=B2=BE=E7=AE=80?= =?UTF-8?q?=E6=97=A0=E7=94=A8=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- 1.Software/PDR 1.05/include/PDRBase.h | 31 +- 1.Software/PDR 1.05/include/Utils.h | 4 +- 1.Software/PDR 1.05/include/pdr_sensor.h | 3 +- 1.Software/PDR 1.05/src/AHRS.c | 404 +++-------------------- 1.Software/PDR 1.05/src/Interface.c | 2 +- 1.Software/PDR 1.05/src/Kalman.c | 282 +--------------- 1.Software/PDR 1.05/src/LinearFit.c | 3 +- 1.Software/PDR 1.05/src/Location.c | 20 +- 1.Software/PDR 1.05/src/Main.cpp | 2 +- 1.Software/PDR 1.05/src/PDRBase.c | 277 ++++++++++++++-- 1.Software/PDR 1.05/src/Utils.c | 72 +--- 11 files changed, 337 insertions(+), 763 deletions(-) diff --git a/1.Software/PDR 1.05/include/PDRBase.h b/1.Software/PDR 1.05/include/PDRBase.h index f6f4643..99e2ed6 100644 --- a/1.Software/PDR 1.05/include/PDRBase.h +++ b/1.Software/PDR 1.05/include/PDRBase.h @@ -143,11 +143,11 @@ typedef struct PDR { double deltaHeading; // 偏航角变化量 double insHeadingOffset; // 惯导方向角偏移 double imuDeltaHeading; // 没间隔一次GPS信号，PDR变化的角度，用于区分转弯 - double gpsHeading; // GPS航向角 - double lastGpsHeading; // 上一次GPS航向角 + double GpsHeading; // GPS航向角 + double LastGpsHeading; // 上一次GPS航向角 double trackHeading; // GPS轨迹航向角 // 速度相关 - double gpsSpeed; // GPS速度 + double GpsSpeed; // GPS速度 // 步数相关 ulong steps; // 当前步数信息 ulong lastSteps; // 上一次的步数 @@ -197,7 +197,7 @@ void calStepLastTime(StepPredict *stepPredict, double timestamp, unsigned long s gnssVel，gps速度 * Output : int， 步数 **************************************************************************/ -int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_last, float gnssVel); +int PredictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_last, float gnssVel); /**---------------------------------------------------------------------- * Function : stateRecognition @@ -207,21 +207,14 @@ int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_ void StateRecognition(float *acc, Classifer_t *sys); -/************************************************************************** -* Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向 -* Input : g_pdr，PDR结构体 - steps，本次步数 - steps_last，上一次步数 - stepPredict，步数预测结构体 - pdr_angle，用户行走方向 - scene_type，GPS用户所处场景（开阔和非开阔） - ss_data， 传感器数据结构体 - pgnss，GPS数据结构体 - kf，EKF结构体 - pdr, PDR预测位置成功标志位 -**************************************************************************/ -void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict,IMU_t *ss_data, - GNSS_t *pgnss, EKFPara_t *kf); +/**---------------------------------------------------------------------- +* Function : InsLocationPredict +* Description : PDR惯导位置更新 +* Date : 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候 +* 会造成PDR前后位置不够均匀 +*---------------------------------------------------------------------**/ +void InsLocationPredict(PDR_t* pdr, StepPredict* stepPredict, IMU_t* imu, + GNSS_t* pgnss, EKFPara_t* kf); diff --git a/1.Software/PDR 1.05/include/Utils.h b/1.Software/PDR 1.05/include/Utils.h index 34fc216..05ceaf7 100644 --- a/1.Software/PDR 1.05/include/Utils.h +++ b/1.Software/PDR 1.05/include/Utils.h @@ -63,11 +63,11 @@ double pow_i(double num, long long n); double pow_f(double num, double m); -double vivopow(double x, double y); +double pow(double x, double y); void centralization(double *x, double *x_out, int n); -void getCovMatrix(double *x, double *y, double cov[2][2], int n); +void GetCovMatrix(double *x, double *y, double cov[2][2], int n); int Jacobi(double a[][2], double p[][2], int n, double eps, int T); diff --git a/1.Software/PDR 1.05/include/pdr_sensor.h b/1.Software/PDR 1.05/include/pdr_sensor.h index 26b14d5..ecb8717 100644 --- a/1.Software/PDR 1.05/include/pdr_sensor.h +++ b/1.Software/PDR 1.05/include/pdr_sensor.h @@ -294,11 +294,12 @@ typedef struct AHRS { uint8_t status; uint8_t stable; // 当前AHRS算法收敛稳定性 float error; - float qnb[4]; // + float q[4]; // float gravity[3]; float x_axis[3]; float y_axis[3]; float z_axis[3]; + float Dt; float Kp; // mahony kp比例调节参数 float Yaw; float Pitch; diff --git a/1.Software/PDR 1.05/src/AHRS.c b/1.Software/PDR 1.05/src/AHRS.c index 9abfc8a..1147048 100644 --- a/1.Software/PDR 1.05/src/AHRS.c +++ b/1.Software/PDR 1.05/src/AHRS.c @@ -21,40 +21,10 @@ #include "Quaternion.h" /* Macro Definition ----------------------------------------------------------*/ -#define AHRS_BUF_LEN 256 -#define FLT_THRES 0.000001f // 浮点数最小阈值 -//计算模值 -#define GET_MOL(v) sqrt((v)[0] * (v)[0] + (v)[1] * (v)[1] + (v)[2] * (v)[2]) -//获取数组长度 -#define ARR_LEN(arr) (sizeof(arr) / sizeof(*arr)) -#define SET_BIT(msk) (g_ahrs.status |= AHRS_STATUS_##msk) -#define CLR_BIT(msk) (g_ahrs.status &= ~AHRS_STATUS_##msk) -#define TST_BIT(msk) (g_ahrs.status & AHRS_STATUS_##msk) -#define INC_PTR(buf, ptr) (((ptr) + 1) % ((buf).length + 1)) - -#define GRV_BUF_NME(i) "grav_buf_##i" -#define GYR_BUF_NME(i) "gyro_buf_##i" -#define MAG_BUF_NME(i) "magn_buf_##i" - +#define FLT_THRES 0.000001f // 浮点数最小阈值 +#define ENABLE_MAG 0 /* Global Variable Definition ------------------------------------------------*/ -/* Macro Definition ----------------------------------------------------------*/ - -/* Global Variable Definition ------------------------------------------------*/ -static IMU g_imu; -AHRS_t g_ahrs; -static uint64_t g_ticker; -static BUFFER_LONG g_erro_buf; -static BUFFER_LONG g_grav_buf[3]; -static BUFFER_LONG g_gyro_buf[3]; -static BUFFER_LONG g_magn_buf[3]; - -static float dt = 1.0f / AHRS_SAMPLE_FREQ; // sample interval in second -static float Kp = 0; // 2 * proportional gain (Kp) -static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // 初始四元数 - -static const float g_x_axis[] = {1, 0, 0}; -static const float g_y_axis[] = {0, 1, 0}; -static const float g_z_axis[] = {0, 0, 1}; +AHRS_t Ahrs; /**---------------------------------------------------------------------- * Function : MahonyUpdateAHRS @@ -65,34 +35,36 @@ static const float g_z_axis[] = {0, 0, 1}; *---------------------------------------------------------------------**/ static void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { + + float* q = Ahrs.q; // 归一化磁力计和加速度计 pdr_v3Norm(&ax, &ay, &az); float wx = 0.0f, wy = 0.0f, wz = 0.0f; - - //if (!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) { +#if ENABLE_MAG + if (!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) { - // pdr_v3Norm(&mx, &my, &mz); + pdr_v3Norm(&mx, &my, &mz); - // // 把磁场从载体系转换到地理坐标系 h = R^-1 * (mx,my,mz) - // float hx = 2.0f * (mx * (0.5f - q2 * q2 - q3 * q3) + my * (q1 * q2 - q0 * q3) + mz * (q1 * q3 + q0 * q2)); - // float hy = 2.0f * (mx * (q1 * q2 + q0 * q3) + my * (0.5f - q1 * q1 - q3 * q3) + mz * (q2 * q3 - q0 * q1)); - // float hz = 2.0f * (mx * (q1 * q3 - q0 * q2) + my * (q2 * q3 + q0 * q1) + mz * (0.5f - q1 * q1 - q2 * q2)); + // 把磁场从载体系转换到地理坐标系 h = R^-1 * (mx,my,mz) + float hx = 2.0f * (mx * (0.5f - q2 * q2 - q[3] * q[3]) + my * (q[1] * q2 - q[0] * q[3]) + mz * (q[1] * q[3] + q[0] * q2)); + float hy = 2.0f * (mx * (q[1] * q2 + q[0] * q[3]) + my * (0.5f - q[1] * q[1] - q[3] * q[3]) + mz * (q2 * q[3] - q[0] * q[1])); + float hz = 2.0f * (mx * (q[1] * q[3] - q[0] * q2) + my * (q2 * q[3] + q[0] * q[1]) + mz * (0.5f - q[1] * q[1] - q2 * q2)); - // // 理论上bx = 0, by 指向北向，因为手机IMU数据使用的是东北天(x,y,z)坐标系 - // float by = (float)sqrt(hx * hx + hy * hy); - // float bz = hz; - - // wx = by * (q0 * q3 + q1 * q2) + bz * (q1 * q3 - q0 * q2); - // wy = by * (0.5f - q1 * q1 - q3 * q3) + bz * (q0 * q1 + q2 * q3); - // wz = by * (q2 * q3 - q0 * q1) + bz * (0.5f - q1 * q1 - q2 * q2); - //} + // 理论上bx = 0, by 指向北向，因为手机IMU数据使用的是东北天(x,y,z)坐标系 + float by = (float)sqrt(hx * hx + hy * hy); + float bz = hz; + wx = by * (q[0] * q[3] + q[1] * q2) + bz * (q[1] * q[3] - q[0] * q2); + wy = by * (0.5f - q[1] * q[1] - q[3] * q[3]) + bz * (q[0] * q[1] + q2 * q[3]); + wz = by * (q2 * q[3] - q[0] * q[1]) + bz * (0.5f - q[1] * q[1] - q2 * q2); + } +#endif // 把重力转换到地理坐标系 v = R * (0,0,1) - float vx = q1*q3 - q0*q2; - float vy = q0*q1 + q2*q3; - float vz = q0*q0 - 0.5f + q3*q3; + float vx = q[1]*q[3] - q[0]*q[2]; + float vy = q[0]*q[1] + q[2]*q[3]; + float vz = q[0]*q[0] - 0.5f + q[3]*q[3]; // 计算矢量叉乘误差 float ex = ay*vz - az*vy + my*wz - mz*wy; @@ -100,323 +72,43 @@ static void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, f float ez = ax*vy - ay*vx + mx*wy - my*wx; // Apply proportional feedback - gx += Kp * ex; - gy += Kp * ey; - gz += Kp * ez; - - // 这两个函数占了大量的时间 - if (g_ahrs.stable == PDR_FALSE) { - BufferPush((Buffer_t*)&g_erro_buf, (float)(2 * sqrt(ex * ex + ey * ey + ez * ez))); - BufferMean((Buffer_t*)&g_erro_buf, &g_ahrs.error); - } + gx += Ahrs.Kp * ex; + gy += Ahrs.Kp * ey; + gz += Ahrs.Kp * ez; // 龙格库塔法更新四元数 - float qa = q0; float qb = q1; float qc = q2; - q0 += (-qb * gx - qc * gy - q3 * gz) * 0.5f * (dt); - q1 += ( qa * gx + qc * gz - q3 * gy) * 0.5f * (dt); - q2 += ( qa * gy - qb * gz + q3 * gx) * 0.5f * (dt); - q3 += ( qa * gz + qb * gy - qc * gx) * 0.5f * (dt); + float qa = q[0]; float qb = q[1]; float qc = q[2]; + q[0] += (-qb * gx - qc * gy - q[3] * gz) * 0.5f * (Ahrs.Dt); + q[1] += ( qa * gx + qc * gz - q[3] * gy) * 0.5f * (Ahrs.Dt); + q[2] += ( qa * gy - qb * gz + q[3] * gx) * 0.5f * (Ahrs.Dt); + q[3] += ( qa * gz + qb * gy - qc * gx) * 0.5f * (Ahrs.Dt); // 四元数归一化 - QuaternionNorm(&q0, &q1, &q2, &q3); + QuaternionNorm(&q[0], &q[1], &q[2], &q[3]); // 四元数转欧拉角 - g_ahrs.Pitch = (float)asin(-2.0f * (q3*q1 - q0*q2)) * (180.0f / 3.141592f); - g_ahrs.Yaw = (float)atan2(q2*q1 + q0*q3, 0.5f - q2*q2 - q3*q3) * (180.0f / 3.141592f); - g_ahrs.Roll = (float)atan2(q2*q3 + q0*q1, 0.5f - q2*q2 - q1*q1) * (180.0f / 3.141592f); -} - -/**--------------------------------------------------------------------- -* Function : AHRS_sensorFrame2EarthFrame -* Description : 转换加速度计值到地球坐标系 -* Date : 2020/7/20 logzhan -*---------------------------------------------------------------------**/ -void AHRS_sensorFrame2EarthFrame(float e[], float s[]) { - float quaVec[4]; - float quaTmp[4]; - float quaCnj[4]; - - quaVec[0] = 0; - quaVec[1] = s[0]; - quaVec[2] = s[1]; - quaVec[3] = s[2]; - QuaternProd(quaTmp, g_ahrs.qnb, quaVec); - QuaternConj(quaCnj, g_ahrs.qnb); - QuaternProd(quaVec, quaTmp, quaCnj); - e[0] = quaVec[1]; - e[1] = quaVec[2]; - e[2] = quaVec[3]; + Ahrs.Pitch = (float)asin(-2.0f * (q[3]*q[1] - q[0]*q[2])) * (180.0f / 3.141592f); + Ahrs.Yaw = (float)atan2(q[2]*q[1] + q[0]*q[3], 0.5f - q[2]*q[2] - q[3]*q[3]) * (180.0f / 3.141592f); + Ahrs.Roll = (float)atan2(q[2]*q[3] + q[0]*q[1], 0.5f - q[2]*q[2] - q[1]*q[1]) * (180.0f / 3.141592f); } void AHRS_Init(void) { - int i; - g_ahrs.stable = PDR_FALSE; - BufferInit((Buffer_t *)&g_erro_buf, "erro_buf", BUFFER_TYPE_QUEUE, AHRS_BUF_LEN); - for (i = 0; i < 3; ++i) { - BufferInit((Buffer_t *)&g_grav_buf[i], GRV_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN); - BufferInit((Buffer_t *)&g_gyro_buf[i], GYR_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN); - BufferInit((Buffer_t *)&g_magn_buf[i], MAG_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN); - } - ResetARHS(); + memset(&Ahrs, 0, sizeof(Ahrs)); + Ahrs.Kp = AHRS_KP; + Ahrs.Dt = 1.0f / AHRS_SAMPLE_FREQ; + Ahrs.q[0] = 1.0f; } -void ResetARHS(void) { - - BufferClear((Buffer_t *)&g_erro_buf); - for (uchar i = 0; i < 3; ++i) { - BufferClear((Buffer_t *)&g_grav_buf[i]); - BufferClear((Buffer_t *)&g_gyro_buf[i]); - BufferClear((Buffer_t *)&g_magn_buf[i]); - } - g_ticker = 0; - q0 = 1.0f; - q1 = 0.0f; - q2 = 0.0f; - q3 = 0.0f; - memset(&g_ahrs, 0, sizeof(g_ahrs)); - g_ahrs.status = (uint8_t)AHRS_STATUS_RESET_PHASE_0; - g_ahrs.error = -1; - g_ahrs.qnb[0] = 1.0f; - g_ahrs.qnb[1] = 0.0f; - g_ahrs.qnb[2] = 0.0f; - g_ahrs.qnb[3] = 0.0f; - g_ahrs.gravity[0] = 0.0f; - g_ahrs.gravity[1] = 0.0f; - g_ahrs.gravity[2] = 9.8f; - g_ahrs.x_axis[0] = 1.0f; - g_ahrs.x_axis[1] = 0.0f; - g_ahrs.x_axis[2] = 0.0f; - g_ahrs.y_axis[0] = 0.0f; - g_ahrs.y_axis[1] = 1.0f; - g_ahrs.y_axis[2] = 0.0f; - g_ahrs.z_axis[0] = 0.0f; - g_ahrs.z_axis[1] = 0.0f; - g_ahrs.z_axis[2] = 1.0f; -} - -/**--------------------------------------------------------------------- -* Function : estimate_sensor_vector -* Description : 利用当前四元数q与陀螺仪，计算在当前姿态下grav -* input : float eVec[], float gyro[] -* output : float sVec[] -* Date : 2020/7/20 logzhan -*---------------------------------------------------------------------**/ -static void estimate_sensor_vector(float sVec[], float eVec[], float gyro[]) { - float q[4]; - float gx, gy, gz; - float qa, qb, qc; - float recipNorm; - float quaVec[4]; - float quaTmp[4]; - float quaCnj[4]; - - /* estimate */ - gx = gyro[0] * (0.5f * (dt)); - gy = gyro[1] * (0.5f * (dt)); - gz = gyro[2] * (0.5f * (dt)); - qa = q0; - qb = q1; - qc = q2; - q[0] = q0 + (-qb * gx - qc * gy - q3 * gz); - q[1] = q1 + (qa * gx + qc * gz - q3 * gy); - q[2] = q2 + (qa * gy - qb * gz + q3 * gx); - q[3] = q3 + (qa * gz + qb * gy - qc * gx); - recipNorm = InvSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); - q[0] *= recipNorm; - q[1] *= recipNorm; - q[2] *= recipNorm; - q[3] *= recipNorm; - - /* project */ - QuaternConj(q, q); - quaVec[0] = 0; - quaVec[1] = eVec[0]; - quaVec[2] = eVec[1]; - quaVec[3] = eVec[2]; - QuaternProd(quaTmp, q, quaVec); - QuaternConj(quaCnj, q); - QuaternProd(quaVec, quaTmp, quaCnj); - sVec[0] = quaVec[1]; - sVec[1] = quaVec[2]; - sVec[2] = quaVec[3]; -} /**---------------------------------------------------------------------- -* Function : pdr_imuUpdateAhrs -* Description : 1、AHRS融合解算 -* 2、Error分析 - * 3、PCA方向计算 -* Date : 2020/6/30 logzhan +* Function : UpdateAHRS +* Description : AHRS融合解算 +* Date : 2022/10/18 logzhan *---------------------------------------------------------------------**/ int UpdateAHRS(IMU_t* imu) { - float gyro[3], acce[3], magn[3], grav[3], mean[2]; - int index; - int count; - int i; - for (uchar i = 0; i < 3; ++i) { - gyro[i] = imu->gyr.s[i]; - acce[i] = imu->acc.s[i]; - magn[i] = imu->mag.s[i]; - } - - //g_ahrs.status(初始状态0x80) 按位与 AHRS_STATUS_RESET_PHASE_0(0x80) - if (g_ahrs.status & AHRS_STATUS_RESET_PHASE_0) { - - // ARHS BUF 中存储了256个sensor的缓存 - if (g_ticker <= AHRS_BUF_LEN) { - for (i = 0; i < 3; ++i) { - BufferPush((Buffer_t *)&g_grav_buf[i], acce[i]); - BufferPush((Buffer_t *)&g_gyro_buf[i], gyro[i]); - BufferPush((Buffer_t *)&g_magn_buf[i], magn[i]); - } - } - // 只有缓存满的时候才计算 - if (g_ticker >= AHRS_BUF_LEN) { - //int index; - index = (g_magn_buf[0]._top + AHRS_BUF_LEN + 1 - AHRS_BUF_LEN / 2) % (g_magn_buf[0].length + 1); - for (i = 0; i < 3; ++i) { - BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]); - magn[i] = g_magn_buf[i].data[index]; - g_grav_buf[i]._top = g_grav_buf[i]._bottom; - } - - Kp = (float)(2 * 2); - /* 这里循环运行1000次,收敛，趋于正确姿态 */ - for (i = 0; i < 1000; ++i) { - MahonyUpdateAHRS(0, 0, 0, grav[0], grav[1], grav[2], magn[0], magn[1], magn[2]); - } - - AHRS_sensorFrame2EarthFrame(g_ahrs.gravity, grav); - - Kp = (float)(2 * 2); - for (index = INC_PTR(g_gyro_buf[0], index); index != g_gyro_buf[0]._top; index = INC_PTR(g_gyro_buf[0], index)) { - for (i = 0; i < 3; ++i) { - BufferPush((Buffer_t *)&g_grav_buf[i], g_ahrs.gravity[i]); - BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]); - gyro[i] = g_gyro_buf[i].data[index]; - acce[i] = g_grav_buf[i].data[index]; - magn[i] = g_magn_buf[i].data[index]; - } - estimate_sensor_vector(grav, grav, gyro); - - // AHRS 更新 - MahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], grav[0], grav[1], grav[2], magn[0], - magn[1], magn[2]); - - AHRS_sensorFrame2EarthFrame(g_ahrs.gravity, acce); - } - - g_ahrs.qnb[0] = q0; - g_ahrs.qnb[1] = q1; - g_ahrs.qnb[2] = q2; - g_ahrs.qnb[3] = q3; - - CLR_BIT(RESET_PHASE_0); /*#define CLR_BIT(msk) (g_ahrs.status &= ~AHRS_STATUS_##msk)；清零g_ahrs.status*/ - SET_BIT(RESET_PHASE_1); /*#define SET_BIT(msk) (g_ahrs.status |= AHRS_STATUS_##msk)；赋值g_ahrs.status，与phase1相同*/ - } - ++g_ticker; - //return; - //g_ahrs.status(0x80) 按位与 AHRS_STATUS_RESET_PHASE_1(0x40) - }else if (g_ahrs.status & AHRS_STATUS_RESET_PHASE_1) { - //PDR_LOGD("阶段1"); - // 应该是判断是否进入PCA的条件 - if (g_ahrs.error < 0.3) { - - BufferCount((Buffer_t *)&g_erro_buf, &count); - memset((void *)mean, 0, sizeof(mean)); - for (index = g_erro_buf._bottom, i = 0; index != g_erro_buf._top; index = INC_PTR(g_erro_buf, index), ++i) { - int c = count >> 1; - if (i < c ) { - mean[0] += g_erro_buf.data[index]; - } - else { - mean[1] += g_erro_buf.data[index]; - } - } - int a = count >> 1; - int b = count - (count >> 1); - mean[0] /= a; - mean[1] /= b; - - if (fabs(mean[0] - mean[1]) < 0.025) { - Kp = (float)(2 * AHRS_KP); - - // 这个标志位决定是否进入PCA方向估计 - CLR_BIT(RESET_PHASE_1); - SET_BIT(STABLE); - g_ahrs.stable = PDR_TRUE; - } - } - - for (i = 0; i < 3; ++i) { - BufferPush((Buffer_t *)&g_grav_buf[i], g_ahrs.gravity[i]); - BufferMean((Buffer_t *)&g_grav_buf[i], &grav[i]); - } - estimate_sensor_vector(grav, grav, gyro); - - MahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], - grav[0], grav[1], grav[2], - magn[0], magn[1], magn[2]); - - g_ahrs.qnb[0] = q0; - g_ahrs.qnb[1] = q1; - g_ahrs.qnb[2] = q2; - g_ahrs.qnb[3] = q3; - - AHRS_sensorFrame2EarthFrame(g_ahrs.gravity, acce); - AHRS_sensorFrame2EarthFrame(g_ahrs.x_axis, (float*)g_x_axis); - AHRS_sensorFrame2EarthFrame(g_ahrs.y_axis, (float*)g_y_axis); - AHRS_sensorFrame2EarthFrame(g_ahrs.z_axis, (float*)g_z_axis); - - //ComputePCA(&g_ahrs); - - ++g_ticker; - } - else { - - // Buffer_mean的计算时间太长了，10ms更新来算，计算256个数的平均 - // 计算量实在太大 - //for (i = 0; i < 3; ++i) { - // Buffer_push((BUFFER *)&g_grav_buf[i], g_ahrs.gravity[i]); - // Buffer_mean((BUFFER *)&g_grav_buf[i], &grav[i]); - //} - //estimate_sensor_vector(grav, grav, gyro); - - // logzhan 21/02/06 : 感觉是acc输入精度会高一些 - MahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], - acce[0], acce[1], acce[2], - magn[0], magn[1], magn[2]); - - g_ahrs.qnb[0] = q0; - g_ahrs.qnb[1] = q1; - g_ahrs.qnb[2] = q2; - g_ahrs.qnb[3] = q3; - - // 把载体系的加速度转换为地理坐标系, - AHRS_sensorFrame2EarthFrame(g_ahrs.gravity, acce); - // 这部分代码都是可以简化以增加运算速度的，暂时不清楚作用 - AHRS_sensorFrame2EarthFrame(g_ahrs.x_axis, (float*)g_x_axis); - AHRS_sensorFrame2EarthFrame(g_ahrs.y_axis, (float*)g_y_axis); - AHRS_sensorFrame2EarthFrame(g_ahrs.z_axis, (float*)g_z_axis); - - ++g_ticker; - return PDR_TRUE; - } - return PDR_FALSE; -} - -/**--------------------------------------------------------------------- -* Function : fv3Norm -* Description : 三维浮点数向量归一化 -* Date : 2021/01/26 logzhan -*---------------------------------------------------------------------**/ -void fv3Norm(float* vx, float* vy, float* vz) -{ - float norm = sqrtf(((*vx) * (*vx) + (*vy) * (*vy) + - (*vz) * (*vz))); - // 防止出现模为0的情况 - if (norm > FLT_THRES) { - norm = 1 / norm; - (*vx) *= norm; (*vy) *= norm; (*vz) *= norm; - } -} + // logzhan 21/02/06 : 感觉是acc输入精度会高一些 + MahonyUpdateAHRS(imu->gyr.s[0], imu->gyr.s[1], imu->gyr.s[2], + imu->acc.s[0], imu->acc.s[1], imu->acc.s[2], + imu->mag.s[0], imu->mag.s[1], imu->mag.s[2]); + return PDR_TRUE; +} \ No newline at end of file diff --git a/1.Software/PDR 1.05/src/Interface.c b/1.Software/PDR 1.05/src/Interface.c index 316c513..e336842 100644 --- a/1.Software/PDR 1.05/src/Interface.c +++ b/1.Software/PDR 1.05/src/Interface.c @@ -25,7 +25,7 @@ IMU_t Imu; /* Extern Variable Definition ------------------------------------------------*/ extern EKFPara_t g_kfPara; -extern AHRS_t g_ahrs; +extern AHRS_t Ahrs; extern GNSS_t pgnss; /* Function Declaration ------------------------------------------------------*/ diff --git a/1.Software/PDR 1.05/src/Kalman.c b/1.Software/PDR 1.05/src/Kalman.c index d558166..37ccb18 100644 --- a/1.Software/PDR 1.05/src/Kalman.c +++ b/1.Software/PDR 1.05/src/Kalman.c @@ -8,36 +8,18 @@ #include #include #include -#if !defined(LOCATION_PLATFORM_QCOM_MODEM) #include -#endif #include "PDRBase.h" #include "Kalman.h" #include "Location.h" #include "Matrix.h" #include "Utils.h" -#include "LinearFit.h" -#include "CoordTrans.h" // 扩展卡尔曼向量缓存 static double EkfVecBuf[5][N] = { {0.0} }; // 扩展卡尔曼矩阵缓存 static double EkfMatBuf[7][N][N] = { { {0.0} } }; -extern double GpsPos[HIST_GPS_NUM][3]; - -extern int debugCount; - - -static int hold_type; -static double b_timestamp; -static double b_last_time; -static double attitude_yaw; -static Detector_t *pDetector; -static int hold_type; -static double BUL_duration; -static double FUR_duration; - /**---------------------------------------------------------------------- * Function : KalmanFilter_Init * Description : 初始化卡尔曼滤波器相关 @@ -45,12 +27,8 @@ static double FUR_duration; *---------------------------------------------------------------------**/ void EKF_Init(void) { - b_timestamp = -1; - b_last_time = -1; - BUL_duration = -1; - FUR_duration = -1; - attitude_yaw = -1; - hold_type = 0; + memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N); + memset(EkfVecBuf, 0, sizeof(double) * 5 * N); } /**---------------------------------------------------------------------- @@ -82,7 +60,6 @@ void EKFStatePredict(EKFPara_t *ekf, double stepLen, PDR_t* pdr, int step) { angle = deltaHeading + ekf->pXk[3]; - if (fabs(deltaHeading) > 5.5) { if (deltaHeading < 0) { deltaHeading = deltaHeading + D2R(360); @@ -136,7 +113,7 @@ void EKFStatePredict(EKFPara_t *ekf, double stepLen, PDR_t* pdr, int step) { * Description : 扩展卡尔曼滤波器的状态更新方程 * Date : 2020/7/22 logzhan *---------------------------------------------------------------------**/ -void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss, Classifer_t *sys, PDR_t *g_pdr, +void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss, Classifer_t *sys, PDR_t *pdr, int scene_type) { double lambda = 0; @@ -261,249 +238,6 @@ void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss, Classifer_t *sys, PDR_t *g_pd } -void calStepLastTime(StepPredict *stepPredict, double timestamp, unsigned long steps_last, - unsigned long steps) -{ - float tmpTime = 0; - if (stepPredict->lastTime > 0 && stepPredict->deltaStep == 0 && (steps - steps_last) > 0) - { - tmpTime = (float)((timestamp - stepPredict->lastTime) / (steps - steps_last)); - if (tmpTime > 300 && tmpTime < 800) - { - if (stepPredict->meanTime > 0){ - stepPredict->meanTime = stepPredict->meanTime * 0.5f + tmpTime * 0.5f; - }else{ - stepPredict->meanTime = tmpTime; - } - } - } - - stepPredict->fnum = 0; - stepPredict->fsum = 0; - - stepPredict->lastTime = timestamp; - stepPredict->deltaStep = 0; - -} - -int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_last, float gnssVel) -{ - int step = 0; - float mean_time = 400; - double dis = 0; - - if (stepPredict->meanTime > 0) - mean_time = stepPredict->meanTime; - - if ((steps_last > 0) && (stepPredict->fnum > 0) && (timestamp - stepPredict->lastTime > mean_time * 3) && (stepPredict->lastTime > 0)) - { - stepPredict->fsum = stepPredict->fsum / stepPredict->fnum; - dis = sqrt(pow(GpsPos[HIST_GPS_NUM - 1][0]- GpsPos[HIST_GPS_NUM - 2][0], 2) + pow(GpsPos[HIST_GPS_NUM - 1][1] - GpsPos[HIST_GPS_NUM - 2][1], 2) - + pow(GpsPos[HIST_GPS_NUM - 1][2] - GpsPos[HIST_GPS_NUM - 2][2], 2)); - - if (stepPredict->fsum != 0.0 && (gnssVel > 1.0 || (dis > 1.0 && dis < 3.0))) - { - step = (int)((timestamp - stepPredict->lastTime) / mean_time); - stepPredict->deltaStep += step; - } - - stepPredict->lastTime = timestamp; - stepPredict->fnum = 0; - stepPredict->fsum = 0; - } - - return step; -} - -/**---------------------------------------------------------------------- -* Function : StateRecognition -* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳 -* Date : 2022/09/19 logzhan -*---------------------------------------------------------------------**/ -void StateRecognition(float *acc, Classifer_t *sys) { - -} - - -double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss) -{ - int i; - double lla[3] = { 0.0 }; - double ned1[3] = { 0.0 }; - double temp[2][HIST_GPS_NUM] = { { 0.0 } }; - double angle[HIST_GPS_NUM - 1] = { 0.0 }; - - double ned2[3] = { 0.0 }; - double maxn = 0; - double maxe = 0; - double minn = 0; - double mine = 0; - double meanx = 0; - double meany = 0; - - float error = 0; - float sst = 0; - float r2 = 0; - - double range = 0; - double yaw = 0; - double tempYaw = 0; - double diffAngle = 0; - - double para[3] = { 0 }; - int flg = 0; - - lla[0] = pgnss.lat; - lla[1] = pgnss.lon; - - for (i = 0; i < HIST_GPS_NUM - 1; i++) - { - gpsPos[i][0] = gpsPos[i + 1][0]; - gpsPos[i][1] = gpsPos[i + 1][1]; - gpsPos[i][2] = gpsPos[i + 1][2]; - } - - if ((lla[0] > 0) && (lla[1] > 0)) - { - //经纬度WGS84转地心地固坐标系ECEF - WGS842ECEF(lla, gpsPos[HIST_GPS_NUM - 1]); - } - else - { - gpsPos[HIST_GPS_NUM - 1][0] = 0; - gpsPos[HIST_GPS_NUM - 1][1] = 0; - gpsPos[HIST_GPS_NUM - 1][2] = 0; - return -1; - } - if (pgnss.error > 40) - { - return -1; - } - - for (i = 0; i < HIST_GPS_NUM - 1; i++) - { - if ((fabs(gpsPos[i][0]) < 1e-6) && (fabs(gpsPos[i][1]) < 1e-6) && (fabs(gpsPos[i][2]) < 1e-6)) - { - return -1; - } - } - //地心地固坐标系ECEF转东北天坐标系NED - ECEF2NED(gpsPos[HIST_GPS_NUM - 1], lla, ned1); - - for (i = HIST_GPS_NUM - 2; i >= 0; i--) { - - ECEF2NED(gpsPos[i], lla, ned2); - - temp[0][i] = ned1[0] - ned2[0]; - temp[1][i] = ned1[1] - ned2[1]; - - meanx += temp[0][i] / HIST_GPS_NUM; - meany += temp[1][i] / HIST_GPS_NUM; - - if (maxn > temp[0][i]) - { - maxn = temp[0][i]; - } - if (minn < temp[0][i]) - { - minn = temp[0][i]; - } - if (maxe > temp[1][i]) - { - maxe = temp[1][i]; - } - if (mine < temp[1][i]) - { - mine = temp[1][i]; - } - - angle[i] = atan2(temp[1][i], temp[0][i]); - if (angle[i] < 0) - angle[i] += TWO_PI; - } - flg = leastDistanceLinearFit(temp[0], temp[1], HIST_GPS_NUM, para); - if (flg < 0) - return -1; - - for (i = HIST_GPS_NUM - 1; i >= 0; i--) { - error += (float)(vivopow(para[0] * temp[0][i] + para[1] * temp[1][i] + para[2], 2) / (vivopow(para[0], 2) + vivopow(para[1], 2))); - sst += (float)(vivopow(meanx - temp[0][i], 2) + vivopow(meany - temp[1][i], 2)); - } - - if (sst > 0) - { - r2 = 1 - error / sst; - } - range = sqrt(vivopow(maxn - minn, 2) + vivopow(maxe - mine, 2)); - //printf("%f,%f,%f\n", r2, error, range); - if (r2 < 0.95 || range < 1.5 || error > 3.0) - return -1; - - yaw = atan(-para[0] / para[1]); - modAngle(&yaw, 0, TWO_PI); - - tempYaw = meanAngle(angle, HIST_GPS_NUM - 1); - modAngle(&tempYaw, 0, TWO_PI); - - diffAngle = tempYaw - yaw; - modAngle(&diffAngle, -PI, PI); - - if (fabs(diffAngle) > PI / 2) - yaw += PI; - modAngle(&yaw, 0, TWO_PI); - - return yaw; -} - -/**---------------------------------------------------------------------- -* Function : InsLocationPredict -* Description : PDR惯导位置更新 -* Date : 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候 -* 会造成PDR前后位置不够均匀 -*---------------------------------------------------------------------**/ -void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict, IMU_t *ss_data, - GNSS_t *pgnss, EKFPara_t *kf) { - int step; - long deltaStep; - double stepLength = 0.7; - - stepPredict->fnum++; - stepPredict->fsum += g_pdr->motionFreq; - - ulong steps = g_pdr->steps; - ulong lastSteps = g_pdr->lastSteps; - - if (steps - lastSteps > 0 && g_pdr->motionFreq != 0.0) { - deltaStep = steps - lastSteps - stepPredict->deltaStep; - - // 应该是防止计步器异常情况，一次计步特别多 - if (deltaStep > 5)deltaStep = 5; - - if (stepPredict->meanTime > 0) - { - step = (int)((ss_data->gyr.time - stepPredict->lastTime) / stepPredict->meanTime + 0.5); - - if (step > deltaStep && step <= 3)deltaStep = step; - } - if (lastSteps == 0)deltaStep = 0; - - if (g_pdr->heading > 0) { - EKFStatePredict(kf, stepLength, g_pdr, deltaStep); - g_pdr->fusionPdrFlg = PDR_TRUE; - } - calStepLastTime(stepPredict, ss_data->gyr.time, lastSteps, steps); - }else{ - // 有时候，计步器可能考虑到稳定性暂时不更新步数，因此需要利用GNSS速度预测步数，达到 - // 惯导更新位置的目的 - int preStep = predictStep(stepPredict, ss_data->gyr.time, lastSteps, pgnss->vel); - if (g_pdr->heading > 0){ - EKFStatePredict(kf, stepLength, g_pdr, preStep); - if(preStep > 0)g_pdr->fusionPdrFlg = PDR_TRUE; - } - } -} - - /**---------------------------------------------------------------------- * Function : EKFCalQRMatrix * Description : 根据GPS信号特征调整卡尔曼滤波噪声矩阵，q矩阵是过程噪声矩阵， @@ -511,11 +245,11 @@ void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict, IMU_t *ss_data, * 信息精度有关。 * Date : 2022/09/19 logzhan *---------------------------------------------------------------------**/ -void EKFCalQRMatrix(Nmea_t *nmea, PDR_t *g_pdr, Classifer_t *sys, +void EKFCalQRMatrix(Nmea_t *nmea, PDR_t *pdr, Classifer_t *sys, EKFPara_t *kf) { - if (g_pdr->MotionType != PDR_MOTION_TYPE_HANDHELD && - g_pdr->MotionType != PDR_MOTION_TYPE_STATIC) { + if (pdr->MotionType != PDR_MOTION_TYPE_HANDHELD && + pdr->MotionType != PDR_MOTION_TYPE_STATIC) { kf->Q[0][0] = 25; // PDR预测位置噪声 kf->Q[1][1] = 25; // PDR预测位置噪声 kf->Q[2][2] = 1; // PDR步长噪声 @@ -548,8 +282,8 @@ void EKFCalQRMatrix(Nmea_t *nmea, PDR_t *g_pdr, Classifer_t *sys, 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) { + if (fabs(R2D((pdr->GpsHeading - pdr->LastGpsHeading))) > 50 || + pdr->GpsSpeed < 0.7 || pdr->GpsHeading == -1) { kf->R[3][3] = 10000; } } diff --git a/1.Software/PDR 1.05/src/LinearFit.c b/1.Software/PDR 1.05/src/LinearFit.c index 9586d7c..cf09331 100644 --- a/1.Software/PDR 1.05/src/LinearFit.c +++ b/1.Software/PDR 1.05/src/LinearFit.c @@ -128,7 +128,8 @@ int leastDistanceLinearFit(double x[], double y[], int num, double para[]) if (num < 2) return -1; - getCovMatrix(x, y, covMatrix, num); + GetCovMatrix(x, y, covMatrix, num); + Jacobi(covMatrix, p, 2, 1e-10, 1000); if (covMatrix[0][0] > covMatrix[1][1]) { diff --git a/1.Software/PDR 1.05/src/Location.c b/1.Software/PDR 1.05/src/Location.c index 5ac4f13..851b0de 100644 --- a/1.Software/PDR 1.05/src/Location.c +++ b/1.Software/PDR 1.05/src/Location.c @@ -30,7 +30,7 @@ GNSS_t pgnss; // EKFPara_t g_kfPara; double GpsPos[HIST_GPS_NUM][3] = {{0}}; static uint32_t g_status; -extern AHRS_t g_ahrs; +extern AHRS_t Ahrs; double refGpsYaw = -1000; double yaw_bias = -1; @@ -158,7 +158,7 @@ int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) { //pdr.heading = CalPredHeading(...); - double imuYaw = - g_ahrs.Yaw; + double imuYaw = - Ahrs.Yaw; if (imuYaw < 0) { imuYaw += 360; } @@ -192,17 +192,17 @@ int GnssInsFusionLocation(Nmea_t *nmea, EKFPara_t *kf, LctFs_t *result) pdr.sceneType = isOpenArea(nmea); // 根据HDOP、速度、卫星数量等判断gps yaw是否可用 - pdr.gpsSpeed = pgnss.vel * GPS_SPEED_UNIT; - pdr.lastGpsHeading = pdr.gpsHeading; - pdr.gpsHeading = GnssCalHeading(&pgnss); + pdr.GpsSpeed = pgnss.vel * GPS_SPEED_UNIT; + pdr.LastGpsHeading = pdr.GpsHeading; + pdr.GpsHeading = GnssCalHeading(&pgnss); // 如果在win32平台下仿真调试,则添加下面代码 #ifdef _WIN32 result->pdrHeading = kf->pXk[3]; - result->gpsHeading = pdr.gpsHeading; + result->gpsHeading = pdr.GpsHeading; result->hdop = pgnss.HDOP; result->accuracy = nmea->accuracy.err; - result->gpsSpeed = pdr.gpsSpeed; + result->gpsSpeed = pdr.GpsSpeed; result->motionType = pdr.MotionType; #endif @@ -304,14 +304,14 @@ void CalPdrHeadingOffset(Nmea_t* nmea_data, PDR_t* p_pdr) { && (DetUserStatic(&pdr, &pgnss, (p_pdr->steps - p_pdr->lastSteps)) == 0 && pdr.MotionType == 2)) { /*这里放姿态角与GPS对准part1 start*/ - double imuHeading = atan2(2 * (g_ahrs.qnb[0] * g_ahrs.qnb[3] + g_ahrs.qnb[1] * g_ahrs.qnb[2]), - 1 - 2 * (g_ahrs.qnb[2] * g_ahrs.qnb[2] + g_ahrs.qnb[3] * g_ahrs.qnb[3])); + double imuHeading = atan2(2 * (Ahrs.q[0] * Ahrs.q[3] + Ahrs.q[1] * Ahrs.q[2]), + 1 - 2 * (Ahrs.q[2] * Ahrs.q[2] + Ahrs.q[3] * Ahrs.q[3])); if (imuHeading < 0) { imuHeading += TWO_PI; } - if (fabs(imuHeading - p_pdr->gpsHeading) < PI / 18) + if (fabs(imuHeading - p_pdr->GpsHeading) < PI / 18) { //printf("offset 标定\n"); //p_pdr->insHeadingOffset = imuHeading - p_pdr->gpsHeading; diff --git a/1.Software/PDR 1.05/src/Main.cpp b/1.Software/PDR 1.05/src/Main.cpp index dd89e7d..4b774a5 100644 --- a/1.Software/PDR 1.05/src/Main.cpp +++ b/1.Software/PDR 1.05/src/Main.cpp @@ -438,7 +438,7 @@ void setSimulatorFileCsvPath(char* path) { * Date : 2020/8/3 logzhan *---------------------------------------------------------------------**/ void setRefGpsYaw() { - refGpsYaw = pdr.gpsHeading; + refGpsYaw = pdr.GpsHeading; printf("ref Gps Yaw = %f\n", refGpsYaw); //g_pdr.sysStatus = IS_INITIAL; //g_pdr.fusionPdrFlg = ON; diff --git a/1.Software/PDR 1.05/src/PDRBase.c b/1.Software/PDR 1.05/src/PDRBase.c index 9a233e6..e2929b6 100644 --- a/1.Software/PDR 1.05/src/PDRBase.c +++ b/1.Software/PDR 1.05/src/PDRBase.c @@ -5,35 +5,254 @@ * Date of Issued : 2022.10.15 * Comments : PDR基础功能函数 ********************************************************************************/ +#include #include "PDRBase.h" +#include "Location.h" +#include "LinearFit.h" +#include "Utils.h" +#include "CoordTrans.h" +#include "Kalman.h" -//extern double GpsPos[HIST_GPS_NUM][3]; +extern double GpsPos[HIST_GPS_NUM][3]; -//int predictStep(StepPredict* stepPredict, double timestamp, unsigned long steps_last, float gnssVel) -//{ -// int step = 0; -// float mean_time = 400; -// double dis = 0; -// -// if (stepPredict->meanTime > 0) -// mean_time = stepPredict->meanTime; -// -// if ((steps_last > 0) && (stepPredict->fnum > 0) && (timestamp - stepPredict->lastTime > mean_time * 3) && (stepPredict->lastTime > 0)) -// { -// stepPredict->fsum = stepPredict->fsum / stepPredict->fnum; -// dis = sqrt(pow(GpsPos[HIST_GPS_NUM - 1][0] - GpsPos[HIST_GPS_NUM - 2][0], 2) + pow(GpsPos[HIST_GPS_NUM - 1][1] - GpsPos[HIST_GPS_NUM - 2][1], 2) -// + pow(GpsPos[HIST_GPS_NUM - 1][2] - GpsPos[HIST_GPS_NUM - 2][2], 2)); -// -// if (stepPredict->fsum != 0.0 && (gnssVel > 1.0 || (dis > 1.0 && dis < 3.0))) -// { -// step = (int)((timestamp - stepPredict->lastTime) / mean_time); -// stepPredict->deltaStep += step; -// } -// -// stepPredict->lastTime = timestamp; -// stepPredict->fnum = 0; -// stepPredict->fsum = 0; -// } -// -// return step; -//} +int PredictStep(StepPredict* stepPredict, double timestamp, unsigned long steps_last, float gnssVel) +{ + int step = 0; + float mean_time = 400; + double dis = 0; + + if (stepPredict->meanTime > 0) + mean_time = stepPredict->meanTime; + + if ((steps_last > 0) && (stepPredict->fnum > 0) && (timestamp - stepPredict->lastTime > mean_time * 3) && (stepPredict->lastTime > 0)) + { + stepPredict->fsum = stepPredict->fsum / stepPredict->fnum; + dis = sqrt(pow(GpsPos[HIST_GPS_NUM - 1][0] - GpsPos[HIST_GPS_NUM - 2][0], 2) + pow(GpsPos[HIST_GPS_NUM - 1][1] - GpsPos[HIST_GPS_NUM - 2][1], 2) + + pow(GpsPos[HIST_GPS_NUM - 1][2] - GpsPos[HIST_GPS_NUM - 2][2], 2)); + + if (stepPredict->fsum != 0.0 && (gnssVel > 1.0 || (dis > 1.0 && dis < 3.0))) + { + step = (int)((timestamp - stepPredict->lastTime) / mean_time); + stepPredict->deltaStep += step; + } + + stepPredict->lastTime = timestamp; + stepPredict->fnum = 0; + stepPredict->fsum = 0; + } + + return step; +} + + +double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss) +{ + int i; + double lla[3] = { 0.0 }; + double ned1[3] = { 0.0 }; + double temp[2][HIST_GPS_NUM] = { { 0.0 } }; + double angle[HIST_GPS_NUM - 1] = { 0.0 }; + + double ned2[3] = { 0.0 }; + double maxn = 0; + double maxe = 0; + double minn = 0; + double mine = 0; + double meanx = 0; + double meany = 0; + + float error = 0; + float sst = 0; + float r2 = 0; + + double range = 0; + double yaw = 0; + double tempYaw = 0; + double diffAngle = 0; + + double para[3] = { 0 }; + int flg = 0; + + lla[0] = pgnss.lat; + lla[1] = pgnss.lon; + + for (i = 0; i < HIST_GPS_NUM - 1; i++) + { + gpsPos[i][0] = gpsPos[i + 1][0]; + gpsPos[i][1] = gpsPos[i + 1][1]; + gpsPos[i][2] = gpsPos[i + 1][2]; + } + // 经纬度WGS84转地心地固坐标系ECEF + if ((lla[0] > 0) && (lla[1] > 0)){ + WGS842ECEF(lla, gpsPos[HIST_GPS_NUM - 1]); + }else{ + gpsPos[HIST_GPS_NUM - 1][0] = 0; + gpsPos[HIST_GPS_NUM - 1][1] = 0; + gpsPos[HIST_GPS_NUM - 1][2] = 0; + return -1; + } + + if (pgnss.error > 40){ + return -1; + } + + for (i = 0; i < HIST_GPS_NUM - 1; i++) + { + if ((fabs(gpsPos[i][0]) < 1e-6) && (fabs(gpsPos[i][1]) < 1e-6) && (fabs(gpsPos[i][2]) < 1e-6)) + { + return -1; + } + } + //地心地固坐标系ECEF转东北天坐标系NED + ECEF2NED(gpsPos[HIST_GPS_NUM - 1], lla, ned1); + + for (i = HIST_GPS_NUM - 2; i >= 0; i--) { + + ECEF2NED(gpsPos[i], lla, ned2); + + temp[0][i] = ned1[0] - ned2[0]; + temp[1][i] = ned1[1] - ned2[1]; + + meanx += temp[0][i] / HIST_GPS_NUM; + meany += temp[1][i] / HIST_GPS_NUM; + + if (maxn > temp[0][i]) + { + maxn = temp[0][i]; + } + if (minn < temp[0][i]) + { + minn = temp[0][i]; + } + if (maxe > temp[1][i]) + { + maxe = temp[1][i]; + } + if (mine < temp[1][i]) + { + mine = temp[1][i]; + } + + angle[i] = atan2(temp[1][i], temp[0][i]); + if (angle[i] < 0) + angle[i] += TWO_PI; + } + flg = leastDistanceLinearFit(temp[0], temp[1], HIST_GPS_NUM, para); + if (flg < 0) + return -1; + + for (i = HIST_GPS_NUM - 1; i >= 0; i--) { + error += (float)(pow(para[0] * temp[0][i] + para[1] * temp[1][i] + para[2], 2) / (pow(para[0], 2) + pow(para[1], 2))); + sst += (float)(pow(meanx - temp[0][i], 2) + pow(meany - temp[1][i], 2)); + } + + if (sst > 0) + { + r2 = 1 - error / sst; + } + range = sqrt(pow(maxn - minn, 2) + pow(maxe - mine, 2)); + //printf("%f,%f,%f\n", r2, error, range); + if (r2 < 0.95 || range < 1.5 || error > 3.0) + return -1; + + yaw = atan(-para[0] / para[1]); + modAngle(&yaw, 0, TWO_PI); + + tempYaw = meanAngle(angle, HIST_GPS_NUM - 1); + modAngle(&tempYaw, 0, TWO_PI); + + diffAngle = tempYaw - yaw; + modAngle(&diffAngle, -PI, PI); + + if (fabs(diffAngle) > PI / 2) + yaw += PI; + modAngle(&yaw, 0, TWO_PI); + + return yaw; +} + +/**---------------------------------------------------------------------- +* Function : StateRecognition +* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳 +* Date : 2022/09/19 logzhan +*---------------------------------------------------------------------**/ +void StateRecognition(float* acc, Classifer_t* sys) { + +} + + + +void calStepLastTime(StepPredict* stepPredict, double timestamp, unsigned long steps_last, + unsigned long steps) +{ + float tmpTime = 0; + if (stepPredict->lastTime > 0 && stepPredict->deltaStep == 0 && (steps - steps_last) > 0) + { + tmpTime = (float)((timestamp - stepPredict->lastTime) / (steps - steps_last)); + if (tmpTime > 300 && tmpTime < 800) + { + if (stepPredict->meanTime > 0) { + stepPredict->meanTime = stepPredict->meanTime * 0.5f + tmpTime * 0.5f; + } + else { + stepPredict->meanTime = tmpTime; + } + } + } + + stepPredict->fnum = 0; + stepPredict->fsum = 0; + + stepPredict->lastTime = timestamp; + stepPredict->deltaStep = 0; + +} + +/**---------------------------------------------------------------------- +* Function : InsLocationPredict +* Description : PDR惯导位置更新 +* Date : 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候 +* 会造成PDR前后位置不够均匀 +*---------------------------------------------------------------------**/ +void InsLocationPredict(PDR_t* pdr, StepPredict* stepPredict, IMU_t* imu, + GNSS_t* pgnss, EKFPara_t* kf) { + int step; + long deltaStep; + double stepLength = 0.7; + + stepPredict->fnum++; + stepPredict->fsum += pdr->motionFreq; + + ulong steps = pdr->steps; + ulong lastSteps = pdr->lastSteps; + + if (steps - lastSteps > 0 && pdr->motionFreq != 0.0) { + deltaStep = steps - lastSteps - stepPredict->deltaStep; + + // 应该是防止计步器异常情况，一次计步特别多 + if (deltaStep > 5)deltaStep = 5; + + if (stepPredict->meanTime > 0) + { + step = (int)((imu->gyr.time - stepPredict->lastTime) / stepPredict->meanTime + 0.5); + + if (step > deltaStep && step <= 3)deltaStep = step; + } + if (lastSteps == 0)deltaStep = 0; + + if (pdr->heading > 0) { + EKFStatePredict(kf, stepLength, pdr, deltaStep); + pdr->fusionPdrFlg = PDR_TRUE; + } + calStepLastTime(stepPredict, imu->gyr.time, lastSteps, steps); + } + else { + // 有时候，计步器可能考虑到稳定性暂时不更新步数，因此需要利用GNSS速度预测步数，达到 + // 惯导更新位置的目的 + int PreStep = PredictStep(stepPredict, imu->gyr.time, lastSteps, pgnss->vel); + if (pdr->heading > 0) { + EKFStatePredict(kf, stepLength, pdr, PreStep); + if (PreStep > 0)pdr->fusionPdrFlg = PDR_TRUE; + } + } +} \ No newline at end of file diff --git a/1.Software/PDR 1.05/src/Utils.c b/1.Software/PDR 1.05/src/Utils.c index 07db071..c7010db 100644 --- a/1.Software/PDR 1.05/src/Utils.c +++ b/1.Software/PDR 1.05/src/Utils.c @@ -154,72 +154,6 @@ const char* Motion2TypeStr(int type) return "UNKOWN"; } -double pow_i(double num, long long n) -{ - double powint = 1; - long long i; - for (i = 1; i <= n; i++) - { - powint *= num; - } - return powint; -} -double pow_f(double num, double m) -{ - int i, j; - double powf = 0, x, tmpm = 1; - x = num - 1; - for (i = 1; tmpm>1e-12 || tmpm<-1e-12; i++) - { - for (j = 1, tmpm = 1; j <= i; j++) - { - tmpm *= (m - j + 1)*x / j; - } - powf += tmpm; - } - return powf + 1; -} - -// https://blog.csdn.net/xiaoxiongli/article/details/2134626 -double vivopow(double num, double m) -{ - if (num == 0 && m != 0) - { - return 0; - } - else if (num == 0 && m == 0) - { - return 1; - } - else if (num < 0 && (m != (long long)m)) - { - return (num - num) / (num - num); - } - - if (num > 2) - { - num = 1 / num; - m = -m; - } - - if (m < 0) - { - return 1 / vivopow(num, -m); - } - - - if (m == (long long)m) - { - return pow_i(num, (long long)m); - } - - else - { - return pow_f(num, m - (long long)m) * pow_i(num, (long long)m); - } -} - - /**---------------------------------------------------------------------- * Function : qnb2att * Description : 四元数转欧拉角 @@ -242,7 +176,7 @@ void Qnb2Att(float* q, double* attitude) -void getCovMatrix(double *x, double *y, double cov[2][2], int n) { +void GetCovMatrix(double *x, double *y, double cov[2][2], int n) { int i; double sum_x = 0, sum_y = 0, sum_xy = 0; double m_x = 0, m_y = 0; @@ -419,9 +353,9 @@ LatLng_t ProjPointOfLatLng(LatLng_t point, LatLng_t linePointA, LatLng_t linePoi else { gradxB_P = point.lat - linePointB.lat; - normPow = vivopow(gradxB_A, 2) + vivopow(gradyB_A, 2); + normPow = pow(gradxB_A, 2) + pow(gradyB_A, 2); resProjPoint.lon = (double)( - (gradxB_P * gradxB_A * gradyB_A + point.lon * vivopow(gradyB_A, 2) + linePointB.lon * vivopow(gradxB_A, 2)) / normPow); + (gradxB_P * gradxB_A * gradyB_A + point.lon * pow(gradyB_A, 2) + linePointB.lon * pow(gradxB_A, 2)) / normPow); if ((fabs(gradxB_A) < DBL_EPSILON) || (fabs(gradyB_A) < DBL_EPSILON)) { /* 当linePointA 和 linePointB 所在直线与 x轴或y轴平行时做规避*/