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

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

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

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

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

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

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

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

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

@ -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 ------------------------------------------------------*/

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

@ -8,36 +8,18 @@
#include <stdio.h>
#include <string.h>
#include <math.h>
#if !defined(LOCATION_PLATFORM_QCOM_MODEM)
#include <malloc.h>
#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;
}
}

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)
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]) {

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

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

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

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

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

@ -5,35 +5,254 @@
* Date of Issued : 2022.10.15
* Comments : PDR基础功能函数
********************************************************************************/
#include <math.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 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;
}
}
}

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

@ -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轴平行时做规避*/