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

大幅度精简无用代码

PDRHistoryBranch
詹力 2022-10-18 13:36:05 +08:00
parent 936e301474
commit 3b19ed22e2
11 changed files with 337 additions and 763 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
1.Software/PDR 1.05/include/PDRBase.h
View File

@ -143,11 +143,11 @@ typedef struct PDR {
double deltaHeading; // 偏航角变化量 double deltaHeading; // 偏航角变化量
double insHeadingOffset; // 惯导方向角偏移 double insHeadingOffset; // 惯导方向角偏移
double imuDeltaHeading; // 没间隔一次GPS信号，PDR变化的角度，用于区分转弯 double imuDeltaHeading; // 没间隔一次GPS信号，PDR变化的角度，用于区分转弯
double gpsHeading; // GPS航向角 double GpsHeading; // GPS航向角
double lastGpsHeading; // 上一次GPS航向角 double LastGpsHeading; // 上一次GPS航向角
double trackHeading; // GPS轨迹航向角 double trackHeading; // GPS轨迹航向角
// 速度相关 // 速度相关
double gpsSpeed; // GPS速度 double GpsSpeed; // GPS速度
// 步数相关 // 步数相关
ulong steps; // 当前步数信息 ulong steps; // 当前步数信息
ulong lastSteps; // 上一次的步数 ulong lastSteps; // 上一次的步数
@ -197,7 +197,7 @@ void calStepLastTime(StepPredict *stepPredict, double timestamp, unsigned long s
gnssVel，gps速度 gnssVel，gps速度
* Output : int， 步数 * 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 * Function : stateRecognition
@ -207,20 +207,13 @@ int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_
void StateRecognition(float *acc, Classifer_t *sys); void StateRecognition(float *acc, Classifer_t *sys);
/************************************************************************** /**----------------------------------------------------------------------
* Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向 * Function : InsLocationPredict
* Input : g_pdr，PDR结构体 * Description : PDR惯导位置更新
steps，本次步数 * Date : 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候
steps_last，上一次步数 * 会造成PDR前后位置不够均匀
stepPredict，步数预测结构体 *---------------------------------------------------------------------**/
pdr_angle，用户行走方向 void InsLocationPredict(PDR_t* pdr, StepPredict* stepPredict, IMU_t* imu,
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); GNSS_t* pgnss, EKFPara_t* kf);

4
1.Software/PDR 1.05/include/Utils.h
View File

@ -63,11 +63,11 @@ double pow_i(double num, long long n);
double pow_f(double num, double m); 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 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); int Jacobi(double a[][2], double p[][2], int n, double eps, int T);

3
1.Software/PDR 1.05/include/pdr_sensor.h
View File

@ -294,11 +294,12 @@ typedef struct AHRS {
uint8_t status; uint8_t status;
uint8_t stable; // 当前AHRS算法收敛稳定性 uint8_t stable; // 当前AHRS算法收敛稳定性
float error; float error;
float qnb[4]; // float q[4]; //
float gravity[3]; float gravity[3];
float x_axis[3]; float x_axis[3];
float y_axis[3]; float y_axis[3];
float z_axis[3]; float z_axis[3];
float Dt;
float Kp; // mahony kp比例调节参数 float Kp; // mahony kp比例调节参数
float Yaw; float Yaw;
float Pitch; float Pitch;

396
1.Software/PDR 1.05/src/AHRS.c
View File

@ -21,40 +21,10 @@
#include "Quaternion.h" #include "Quaternion.h"
/* Macro Definition ----------------------------------------------------------*/ /* Macro Definition ----------------------------------------------------------*/
#define AHRS_BUF_LEN 256
#define FLT_THRES 0.000001f // 浮点数最小阈值 #define FLT_THRES 0.000001f // 浮点数最小阈值
//计算模值 #define ENABLE_MAG 0
#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"
/* Global Variable Definition ------------------------------------------------*/ /* Global Variable Definition ------------------------------------------------*/
/* Macro Definition ----------------------------------------------------------*/ AHRS_t Ahrs;
/* 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};
/**---------------------------------------------------------------------- /**----------------------------------------------------------------------
* Function : MahonyUpdateAHRS * 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, static void MahonyUpdateAHRS(float gx, float gy, float gz, float ax, float ay, float az,
float mx, float my, float mz) { float mx, float my, float mz) {
float* q = Ahrs.q;
// 归一化磁力计和加速度计 // 归一化磁力计和加速度计
pdr_v3Norm(&ax, &ay, &az); pdr_v3Norm(&ax, &ay, &az);
float wx = 0.0f, wy = 0.0f, wz = 0.0f; float wx = 0.0f, wy = 0.0f, wz = 0.0f;
#if ENABLE_MAG
if (!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
//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 - 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));
// // 把磁场从载体系转换到地理坐标系 h = R^-1 * (mx,my,mz) // 理论上bx = 0, by 指向北向，因为手机IMU数据使用的是东北天(x,y,z)坐标系
// float hx = 2.0f * (mx * (0.5f - q2 * q2 - q3 * q3) + my * (q1 * q2 - q0 * q3) + mz * (q1 * q3 + q0 * q2)); float by = (float)sqrt(hx * hx + hy * hy);
// float hy = 2.0f * (mx * (q1 * q2 + q0 * q3) + my * (0.5f - q1 * q1 - q3 * q3) + mz * (q2 * q3 - q0 * q1)); float bz = hz;
// float hz = 2.0f * (mx * (q1 * q3 - q0 * q2) + my * (q2 * q3 + q0 * q1) + mz * (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 * (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);
//}
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) // 把重力转换到地理坐标系 v = R * (0,0,1)
float vx = q1*q3 - q0*q2; float vx = q[1]*q[3] - q[0]*q[2];
float vy = q0*q1 + q2*q3; float vy = q[0]*q[1] + q[2]*q[3];
float vz = q0*q0 - 0.5f + q3*q3; float vz = q[0]*q[0] - 0.5f + q[3]*q[3];
// 计算矢量叉乘误差 // 计算矢量叉乘误差
float ex = ay*vz - az*vy + my*wz - mz*wy; 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; float ez = ax*vy - ay*vx + mx*wy - my*wx;
// Apply proportional feedback // Apply proportional feedback
gx += Kp * ex; gx += Ahrs.Kp * ex;
gy += Kp * ey; gy += Ahrs.Kp * ey;
gz += Kp * ez; gz += Ahrs.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);
}
// 龙格库塔法更新四元数 // 龙格库塔法更新四元数
float qa = q0; float qb = q1; float qc = q2; float qa = q[0]; float qb = q[1]; float qc = q[2];
q0 += (-qb * gx - qc * gy - q3 * gz) * 0.5f * (dt); q[0] += (-qb * gx - qc * gy - q[3] * gz) * 0.5f * (Ahrs.Dt);
q1 += ( qa * gx + qc * gz - q3 * gy) * 0.5f * (dt); q[1] += ( qa * gx + qc * gz - q[3] * gy) * 0.5f * (Ahrs.Dt);
q2 += ( qa * gy - qb * gz + q3 * gx) * 0.5f * (dt); q[2] += ( qa * gy - qb * gz + q[3] * gx) * 0.5f * (Ahrs.Dt);
q3 += ( qa * gz + qb * gy - qc * gx) * 0.5f * (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); Ahrs.Pitch = (float)asin(-2.0f * (q[3]*q[1] - q[0]*q[2])) * (180.0f / 3.141592f);
g_ahrs.Yaw = (float)atan2(q2*q1 + q0*q3, 0.5f - q2*q2 - q3*q3) * (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);
g_ahrs.Roll = (float)atan2(q2*q3 + q0*q1, 0.5f - q2*q2 - q1*q1) * (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);
}
/**---------------------------------------------------------------------
* 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];
} }
void AHRS_Init(void) { void AHRS_Init(void) {
int i; memset(&Ahrs, 0, sizeof(Ahrs));
g_ahrs.stable = PDR_FALSE; Ahrs.Kp = AHRS_KP;
BufferInit((Buffer_t *)&g_erro_buf, "erro_buf", BUFFER_TYPE_QUEUE, AHRS_BUF_LEN); Ahrs.Dt = 1.0f / AHRS_SAMPLE_FREQ;
for (i = 0; i < 3; ++i) { Ahrs.q[0] = 1.0f;
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();
} }
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 * Function : UpdateAHRS
* Description : 1、AHRS融合解算 * Description : AHRS融合解算
* 2、Error分析 * Date : 2022/10/18 logzhan
* 3、PCA方向计算
* Date : 2020/6/30 logzhan
*---------------------------------------------------------------------**/ *---------------------------------------------------------------------**/
int UpdateAHRS(IMU_t* imu) { 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输入精度会高一些 // logzhan 21/02/06 : 感觉是acc输入精度会高一些
MahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], MahonyUpdateAHRS(imu->gyr.s[0], imu->gyr.s[1], imu->gyr.s[2],
acce[0], acce[1], acce[2], imu->acc.s[0], imu->acc.s[1], imu->acc.s[2],
magn[0], magn[1], magn[2]); imu->mag.s[0], imu->mag.s[1], imu->mag.s[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_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;
}
}

2
1.Software/PDR 1.05/src/Interface.c
View File

@ -25,7 +25,7 @@ IMU_t Imu;
/* Extern Variable Definition ------------------------------------------------*/ /* Extern Variable Definition ------------------------------------------------*/
extern EKFPara_t g_kfPara; extern EKFPara_t g_kfPara;
extern AHRS_t g_ahrs; extern AHRS_t Ahrs;
extern GNSS_t pgnss; extern GNSS_t pgnss;
/* Function Declaration ------------------------------------------------------*/ /* Function Declaration ------------------------------------------------------*/

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

@ -8,36 +8,18 @@
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
#include <math.h> #include <math.h>
#if !defined(LOCATION_PLATFORM_QCOM_MODEM)
#include <malloc.h> #include <malloc.h>
#endif
#include "PDRBase.h" #include "PDRBase.h"
#include "Kalman.h" #include "Kalman.h"
#include "Location.h" #include "Location.h"
#include "Matrix.h" #include "Matrix.h"
#include "Utils.h" #include "Utils.h"
#include "LinearFit.h"
#include "CoordTrans.h"
// 扩展卡尔曼向量缓存 // 扩展卡尔曼向量缓存
static double EkfVecBuf[5][N] = { {0.0} }; static double EkfVecBuf[5][N] = { {0.0} };
// 扩展卡尔曼矩阵缓存 // 扩展卡尔曼矩阵缓存
static double EkfMatBuf[7][N][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 * Function : KalmanFilter_Init
* Description : 初始化卡尔曼滤波器相关 * Description : 初始化卡尔曼滤波器相关
@ -45,12 +27,8 @@ static double FUR_duration;
*---------------------------------------------------------------------**/ *---------------------------------------------------------------------**/
void EKF_Init(void) void EKF_Init(void)
{ {
b_timestamp = -1; memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
b_last_time = -1; memset(EkfVecBuf, 0, sizeof(double) * 5 * N);
BUL_duration = -1;
FUR_duration = -1;
attitude_yaw = -1;
hold_type = 0;
} }
/**---------------------------------------------------------------------- /**----------------------------------------------------------------------
@ -82,7 +60,6 @@ void EKFStatePredict(EKFPara_t *ekf, double stepLen, PDR_t* pdr, int step) {
angle = deltaHeading + ekf->pXk[3]; angle = deltaHeading + ekf->pXk[3];
if (fabs(deltaHeading) > 5.5) { if (fabs(deltaHeading) > 5.5) {
if (deltaHeading < 0) { if (deltaHeading < 0) {
deltaHeading = deltaHeading + D2R(360); deltaHeading = deltaHeading + D2R(360);
@ -136,7 +113,7 @@ void EKFStatePredict(EKFPara_t *ekf, double stepLen, PDR_t* pdr, int step) {
* Description : 扩展卡尔曼滤波器的状态更新方程 * Description : 扩展卡尔曼滤波器的状态更新方程
* Date : 2020/7/22 logzhan * 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) { int scene_type) {
double lambda = 0; 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 * Function : EKFCalQRMatrix
* Description : 根据GPS信号特征调整卡尔曼滤波噪声矩阵，q矩阵是过程噪声矩阵， * Description : 根据GPS信号特征调整卡尔曼滤波噪声矩阵，q矩阵是过程噪声矩阵，
@ -511,11 +245,11 @@ void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict, IMU_t *ss_data,
* 信息精度有关。 * 信息精度有关。
* Date : 2022/09/19 logzhan * 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) { EKFPara_t *kf) {
if (g_pdr->MotionType != PDR_MOTION_TYPE_HANDHELD && if (pdr->MotionType != PDR_MOTION_TYPE_HANDHELD &&
g_pdr->MotionType != PDR_MOTION_TYPE_STATIC) { pdr->MotionType != PDR_MOTION_TYPE_STATIC) {
kf->Q[0][0] = 25; // PDR预测位置噪声 kf->Q[0][0] = 25; // PDR预测位置噪声
kf->Q[1][1] = 25; // PDR预测位置噪声 kf->Q[1][1] = 25; // PDR预测位置噪声
kf->Q[2][2] = 1; // 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方向噪声 kf->R[3][3] = 50; // GPS方向噪声
} }
if (fabs(R2D((g_pdr->gpsHeading - g_pdr->lastGpsHeading))) > 50 || if (fabs(R2D((pdr->GpsHeading - pdr->LastGpsHeading))) > 50 ||
g_pdr->gpsSpeed < 0.7 || g_pdr->gpsHeading == -1) { pdr->GpsSpeed < 0.7 || pdr->GpsHeading == -1) {
kf->R[3][3] = 10000; kf->R[3][3] = 10000;
} }
} }

3
1.Software/PDR 1.05/src/LinearFit.c
View File

@ -128,7 +128,8 @@ int leastDistanceLinearFit(double x[], double y[], int num, double para[])
if (num < 2) if (num < 2)
return -1; return -1;
getCovMatrix(x, y, covMatrix, num); GetCovMatrix(x, y, covMatrix, num);
Jacobi(covMatrix, p, 2, 1e-10, 1000); Jacobi(covMatrix, p, 2, 1e-10, 1000);
if (covMatrix[0][0] > covMatrix[1][1]) { if (covMatrix[0][0] > covMatrix[1][1]) {

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

@ -30,7 +30,7 @@ GNSS_t pgnss; //
EKFPara_t g_kfPara; EKFPara_t g_kfPara;
double GpsPos[HIST_GPS_NUM][3] = {{0}}; double GpsPos[HIST_GPS_NUM][3] = {{0}};
static uint32_t g_status; static uint32_t g_status;
extern AHRS_t g_ahrs; extern AHRS_t Ahrs;
double refGpsYaw = -1000; double refGpsYaw = -1000;
double yaw_bias = -1; double yaw_bias = -1;
@ -158,7 +158,7 @@ int InsLocation(IMU_t* ImuData, EKFPara_t* Ekf) {
//pdr.heading = CalPredHeading(...); //pdr.heading = CalPredHeading(...);
double imuYaw = - g_ahrs.Yaw; double imuYaw = - Ahrs.Yaw;
if (imuYaw < 0) { if (imuYaw < 0) {
imuYaw += 360; imuYaw += 360;
} }
@ -192,17 +192,17 @@ int GnssInsFusionLocation(Nmea_t *nmea, EKFPara_t *kf, LctFs_t *result)
pdr.sceneType = isOpenArea(nmea); pdr.sceneType = isOpenArea(nmea);
// 根据HDOP、速度、卫星数量等判断gps yaw是否可用 // 根据HDOP、速度、卫星数量等判断gps yaw是否可用
pdr.gpsSpeed = pgnss.vel * GPS_SPEED_UNIT; pdr.GpsSpeed = pgnss.vel * GPS_SPEED_UNIT;
pdr.lastGpsHeading = pdr.gpsHeading; pdr.LastGpsHeading = pdr.GpsHeading;
pdr.gpsHeading = GnssCalHeading(&pgnss); pdr.GpsHeading = GnssCalHeading(&pgnss);
// 如果在win32平台下仿真调试,则添加下面代码 // 如果在win32平台下仿真调试,则添加下面代码
#ifdef _WIN32 #ifdef _WIN32
result->pdrHeading = kf->pXk[3]; result->pdrHeading = kf->pXk[3];
result->gpsHeading = pdr.gpsHeading; result->gpsHeading = pdr.GpsHeading;
result->hdop = pgnss.HDOP; result->hdop = pgnss.HDOP;
result->accuracy = nmea->accuracy.err; result->accuracy = nmea->accuracy.err;
result->gpsSpeed = pdr.gpsSpeed; result->gpsSpeed = pdr.GpsSpeed;
result->motionType = pdr.MotionType; result->motionType = pdr.MotionType;
#endif #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)) && (DetUserStatic(&pdr, &pgnss, (p_pdr->steps - p_pdr->lastSteps)) == 0 && pdr.MotionType == 2))
{ {
/*这里放姿态角与GPS对准part1 start*/ /*这里放姿态角与GPS对准part1 start*/
double imuHeading = atan2(2 * (g_ahrs.qnb[0] * g_ahrs.qnb[3] + g_ahrs.qnb[1] * g_ahrs.qnb[2]), double imuHeading = atan2(2 * (Ahrs.q[0] * Ahrs.q[3] + Ahrs.q[1] * Ahrs.q[2]),
1 - 2 * (g_ahrs.qnb[2] * g_ahrs.qnb[2] + g_ahrs.qnb[3] * g_ahrs.qnb[3])); 1 - 2 * (Ahrs.q[2] * Ahrs.q[2] + Ahrs.q[3] * Ahrs.q[3]));
if (imuHeading < 0) { if (imuHeading < 0) {
imuHeading += TWO_PI; imuHeading += TWO_PI;
} }
if (fabs(imuHeading - p_pdr->gpsHeading) < PI / 18) if (fabs(imuHeading - p_pdr->GpsHeading) < PI / 18)
{ {
//printf("offset 标定\n"); //printf("offset 标定\n");
//p_pdr->insHeadingOffset = imuHeading - p_pdr->gpsHeading; //p_pdr->insHeadingOffset = imuHeading - p_pdr->gpsHeading;

2
1.Software/PDR 1.05/src/Main.cpp
View File

@ -438,7 +438,7 @@ void setSimulatorFileCsvPath(char* path) {
* Date : 2020/8/3 logzhan * Date : 2020/8/3 logzhan
*---------------------------------------------------------------------**/ *---------------------------------------------------------------------**/
void setRefGpsYaw() { void setRefGpsYaw() {
refGpsYaw = pdr.gpsHeading; refGpsYaw = pdr.GpsHeading;
printf("ref Gps Yaw = %f\n", refGpsYaw); printf("ref Gps Yaw = %f\n", refGpsYaw);
//g_pdr.sysStatus = IS_INITIAL; //g_pdr.sysStatus = IS_INITIAL;
//g_pdr.fusionPdrFlg = ON; //g_pdr.fusionPdrFlg = ON;

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

@ -5,35 +5,254 @@
* Date of Issued : 2022.10.15 * Date of Issued : 2022.10.15
* Comments : PDR基础功能函数 * Comments : PDR基础功能函数
********************************************************************************/ ********************************************************************************/
#include <math.h>
#include "PDRBase.h" #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 PredictStep(StepPredict* stepPredict, double timestamp, unsigned long steps_last, float gnssVel)
//{ {
// int step = 0; int step = 0;
// float mean_time = 400; float mean_time = 400;
// double dis = 0; double dis = 0;
//
// if (stepPredict->meanTime > 0) if (stepPredict->meanTime > 0)
// mean_time = stepPredict->meanTime; mean_time = stepPredict->meanTime;
//
// if ((steps_last > 0) && (stepPredict->fnum > 0) && (timestamp - stepPredict->lastTime > mean_time * 3) && (stepPredict->lastTime > 0)) if ((steps_last > 0) && (stepPredict->fnum > 0) && (timestamp - stepPredict->lastTime > mean_time * 3) && (stepPredict->lastTime > 0))
// { {
// stepPredict->fsum = stepPredict->fsum / stepPredict->fnum; 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) 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)); + 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))) if (stepPredict->fsum != 0.0 && (gnssVel > 1.0 || (dis > 1.0 && dis < 3.0)))
// { {
// step = (int)((timestamp - stepPredict->lastTime) / mean_time); step = (int)((timestamp - stepPredict->lastTime) / mean_time);
// stepPredict->deltaStep += step; stepPredict->deltaStep += step;
// } }
//
// stepPredict->lastTime = timestamp; stepPredict->lastTime = timestamp;
// stepPredict->fnum = 0; stepPredict->fnum = 0;
// stepPredict->fsum = 0; stepPredict->fsum = 0;
// } }
//
// return step; 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;
}
}
}

72
1.Software/PDR 1.05/src/Utils.c
View File

@ -154,72 +154,6 @@ const char* Motion2TypeStr(int type)
return "UNKOWN"; 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 * Function : qnb2att
* Description : 四元数转欧拉角 * 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; int i;
double sum_x = 0, sum_y = 0, sum_xy = 0; double sum_x = 0, sum_y = 0, sum_xy = 0;
double m_x = 0, m_y = 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 else
{ {
gradxB_P = point.lat - linePointB.lat; 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)( 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)) if ((fabs(gradxB_A) < DBL_EPSILON) || (fabs(gradyB_A) < DBL_EPSILON))
{ {
/* 当linePointA 和 linePointB 所在直线与 x轴或y轴平行时做规避*/ /* 当linePointA 和 linePointB 所在直线与 x轴或y轴平行时做规避*/