大幅度精简无用代码
parent
936e301474
commit
3b19ed22e2
|
|
@ -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,20 +207,13 @@ 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,
|
||||
/**----------------------------------------------------------------------
|
||||
* 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);
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 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 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)
|
||||
// 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));
|
||||
|
||||
// // 理论上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;
|
||||
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;
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 ------------------------------------------------------*/
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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]) {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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轴平行时做规避*/
|
||||
|
|
|
|||
Loading…
Reference in New Issue