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

重构函数命名

PDRHistoryBranch
詹力 2022-09-19 23:05:00 +08:00
parent fe1084e9db
commit 2790501743
17 changed files with 92 additions and 305 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
1.Software/PDR 1.02/include/pdr_ahrs.h
View File

@ -27,7 +27,7 @@ extern "C" {
/* Function Declaration ------------------------------------------------------------------------- */
void AHRS_Init(void);
void pdr_ahrsReset(void);
void ResetARHS(void);
/**---------------------------------------------------------------------
* Function : fv3Norm

8
1.Software/PDR 1.02/include/pdr_base.h
View File

@ -234,7 +234,7 @@ int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_
* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳
* Date : 2020/7/22 logzhan
*---------------------------------------------------------------------**/
void stateRecognition(float *acc, classifer *sys);
void StateRecognition(float *acc, classifer *sys);
/**************************************************************************
* Description : 根据GPS角度、GPS轨迹角、PDR角度，计算用户行走方向
@ -260,7 +260,7 @@ double calPredAngle(PDR *g_pdr, classifer *sys, double gps_yaw, double G_yaw, im
kf，EKF结构体
pdr, PDR预测位置成功标志位
**************************************************************************/
void pdr_insPredict(PDR *g_pdr, StepPredict *stepPredict,imu *ss_data,
void InsPredict(PDR *g_pdr, StepPredict *stepPredict,imu *ss_data,
GNSS_t *pgnss, KfPara_t *kf);
/**----------------------------------------------------------------------
@ -288,7 +288,7 @@ void OutputOriginGnssPos(GNSS_t *pgnss, lct_fs *result);
* kf， EKF数据结构体
* Date : 2020/07/08 logzhan
*---------------------------------------------------------------------**/
int resetOutputResult(GNSS_t *pgnss, PDR* g_pdr, KfPara_t *kf, lct_fs *result);
int ResetOutputResult(GNSS_t *pgnss, PDR* g_pdr, KfPara_t *kf, lct_fs *result);
/**----------------------------------------------------------------------
* Function : detIsCarMode
@ -337,7 +337,7 @@ int pdr_initProc(GNSS_t *pgnss, KfPara_t *kf, PDR* g_pdr, lct_fs *result);
pgnss，GPS数据结构体
* Output : double，轨迹角
**************************************************************************/
double calGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss);
double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss);
#endif

2
1.Software/PDR 1.02/include/pdr_detector.h
View File

@ -76,7 +76,7 @@ void mag_min(float a[MAG_BUF_LEN], float *mag_min);
void mag_max(float a[MAG_BUF_LEN], float *mag_max);
void mag_rand(float r[6]);
void updateDetectorImu(imu* imu);
void UpdateDetectorImu(imu* imu);
#ifdef __cplusplus
}

23
1.Software/PDR 1.02/include/pdr_kalman.h
View File

@ -14,31 +14,30 @@ extern "C" {
#endif
/**----------------------------------------------------------------------
* Function : pdr_initKalman
* Function : EKF_Init
* Description : 初始化卡尔曼滤波器相关
* Date : 2020/8/27
*
* Date : 2022/09/19
*---------------------------------------------------------------------**/
void KalmanFilter_Init(void);
void EKF_Init(void);
/**----------------------------------------------------------------------
* Function : pdr_ekfStatePredict
* Function : EKFStatePredict
* Description : pdr卡尔曼滤波器的状态预测方程
* Date : 2020/7/22 logzhan
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
void pdr_ekfStatePredict(KfPara_t* kf, double step_length, PDR* g_pdr, int step);
void EKFStatePredict(KfPara_t* kf, double step_length, PDR* g_pdr, int step);
/**----------------------------------------------------------------------
* Function : getQRValue
* Description : µ÷Õû¿¨ûÂüÂ˲¨ÔëÉù¾ØÕó
* Function : EKFCalQRMatrix
* Description : ¼ÆË㿨ûÂüÂ˲¨ÔëÉù¾ØÕó
* scane_type, 用户所处场景（开阔和非开阔）
* nmea_data，NMEA数据结构体
* g_pdr，PDR结构体
* sys，运动幅度数据结构体
* kf，EKF数据结构体
* Date : 2020/07/09 logzhan
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
void CalEKFQRMatrix(lct_nmea* nmea_data, PDR* g_pdr, classifer* sys, KfPara_t* kf);
void EKFCalQRMatrix(lct_nmea* nmea_data, PDR* g_pdr, classifer* sys, KfPara_t* kf);
/**----------------------------------------------------------------------
* Function : pdr_ekfStateUpdate
@ -53,7 +52,7 @@ void EKFStateUpdate(KfPara_t* kf, GNSS_t* pgnss, classifer* sys, PDR* g_pdr,
* Description : PDR的GNSS和INS融合定位
* Date : 2020/07/09 logzhan
*---------------------------------------------------------------------**/
void pdr_gnssInsLocFusion(GNSS_t* pgnss, PDR* g_pdr, classifer* sys, double yaw_bias,
void GnssInsLocFusion(GNSS_t* pgnss, PDR* g_pdr, classifer* sys, double yaw_bias,
KfPara_t* kf, lct_fs* res);
#ifdef __cplusplus

4
1.Software/PDR 1.02/include/pdr_location.h
View File

@ -50,7 +50,7 @@ void Nmea2Gnss(lct_nmea* nmea_data, GNSS_t* pgnss);
* 2020/02/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高
* 代码的可读性
*---------------------------------------------------------------------**/
int pdr_detectFixMode(GNSS_t* pgnss, KfPara_t* kf, PDR* g_pdr, lct_fs* result);
int DetectFixMode(GNSS_t* pgnss, KfPara_t* kf, PDR* g_pdr, lct_fs* result);
/**----------------------------------------------------------------------
* Function : GnssCalHeading
@ -65,7 +65,7 @@ double GnssCalHeading(GNSS_t* pgnss);
* Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移
* Date : 2020/07/08 logzhan
*---------------------------------------------------------------------**/
void calPdrHeadingOffset(lct_nmea* nmea_data, PDR* p_pdr);
void CalPdrHeadingOffset(lct_nmea* nmea_data, PDR* p_pdr);
/**---------------------------------------------------------------------
* Function : pdr_resetSysStatus

2
1.Software/PDR 1.02/include/pdr_main.h
View File

@ -76,7 +76,7 @@ const char* Motion2TypeStr(int type);
* Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹
* Date : 2021/01/25 logzhan
*---------------------------------------------------------------------**/
void updateResTrack(ResultTracks& resTrack, lct_fs& lctfs);
void UpdateResTrack(ResultTracks& resTrack, lct_fs& lctfs);
#endif

4
1.Software/PDR 1.02/include/pdr_util.h
View File

@ -83,9 +83,9 @@ void ECEF2Ned(double *ecef, double *plla, double *ned);
void NED2ECEF(double *plla, double *ned, double *ecef0, double *ecef);
void Wgs842Ned(double *plla, double *ref_lla, double *ned);
void WGS842NED(double *plla, double *ref_lla, double *ned);
void ned2Wgs84(double *ref_plla, double *ned, double* plla);
void NED2WGS84(double *ref_plla, double *ned, double* plla);
LatLng ProjPointOfLatLng(LatLng point, LatLng linePointA, LatLng linePointB);

2
1.Software/PDR 1.02/include/steps.h
View File

@ -160,7 +160,7 @@ void reinitPedometer(void);
* Description : PDR 计步器的加速度输入, 并通过指针返回用户步数
* Date : 2020/2/1 logzhan
*---------------------------------------------------------------------**/
void updatePedometer(imu* ss_data, unsigned long* step);
void UpdatePedometer(imu* ss_data, unsigned long* step);
void Steps_State_Detect(enumState* state);

6
1.Software/PDR 1.02/project/PDR.vcxproj.filters
View File

@ -104,9 +104,6 @@
<ClInclude Include="..\include\pdr_fft.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\pdr_kalman.h">
<Filter>inc</Filter>
</ClInclude>
<ClInclude Include="..\include\pdr_linearFit.h">
<Filter>inc</Filter>
</ClInclude>
@ -134,5 +131,8 @@
<ClInclude Include="..\include\pdr_util.h">
<Filter>src\Utils</Filter>
</ClInclude>
<ClInclude Include="..\include\pdr_kalman.h">
<Filter>src\Location</Filter>
</ClInclude>
</ItemGroup>
</Project>

21
1.Software/PDR 1.02/src/pdr_ahrs.c
View File

@ -56,14 +56,13 @@ static const float g_y_axis[] = {0, 1, 0};
static const float g_z_axis[] = {0, 0, 1};
/**----------------------------------------------------------------------
* Function : mahonyUpdateAHRS
* Description : mahony×Ë̬¸üÐÂËã·¨
* Date : 2020/8/3 logzhan
* 2020/02/18 logzhan :
* Function : MahonyUpdateAHRS
* Description : Mahony×Ë̬¸üÐÂËã·¨
* Date : 2020/02/18 logzhan :
* 增加了AHRS稳定性标志位，如果AHRS稳定后不再计算error, 降低运算
量。能提升PDR仿真10%的运行速度
*---------------------------------------------------------------------**/
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) {
// 归一化磁力计和加速度计
pdr_v3Norm(&ax, &ay, &az);
@ -157,10 +156,10 @@ void AHRS_Init(void) {
Buffer_initialize((BUFFER *)&g_gyro_buf[i], GYR_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN);
Buffer_initialize((BUFFER *)&g_magn_buf[i], MAG_BUF_NME(i), BUFFER_TYPE_QUEUE, AHRS_BUF_LEN);
}
pdr_ahrsReset();
ResetARHS();
}
void pdr_ahrsReset(void) {
void ResetARHS(void) {
Buffer_clear((BUFFER *)&g_erro_buf);
for (uchar i = 0; i < 3; ++i) {
@ -283,7 +282,7 @@ int UpdateAHRS(imu* imu) {
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]);
MahonyUpdateAHRS(0, 0, 0, grav[0], grav[1], grav[2], magn[0], magn[1], magn[2]);
}
AHRS_sensorFrame2EarthFrame(g_ahrs.gravity, grav);
@ -300,7 +299,7 @@ int UpdateAHRS(imu* imu) {
estimate_sensor_vector(grav, grav, gyro);
// AHRS 更新
mahonyUpdateAHRS(gyro[0], gyro[1], gyro[2], grav[0], grav[1], grav[2], magn[0],
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);
@ -354,7 +353,7 @@ int UpdateAHRS(imu* imu) {
}
estimate_sensor_vector(grav, grav, gyro);
mahonyUpdateAHRS(gyro[0], gyro[1], gyro[2],
MahonyUpdateAHRS(gyro[0], gyro[1], gyro[2],
grav[0], grav[1], grav[2],
magn[0], magn[1], magn[2]);
@ -383,7 +382,7 @@ int UpdateAHRS(imu* imu) {
//estimate_sensor_vector(grav, grav, gyro);
// logzhan 21/02/06 : 感觉是acc输入精度会高一些
mahonyUpdateAHRS(gyro[0], gyro[1], gyro[2],
MahonyUpdateAHRS(gyro[0], gyro[1], gyro[2],
acce[0], acce[1], acce[2],
magn[0], magn[1], magn[2]);

159
1.Software/PDR 1.02/src/pdr_base.c
View File

@ -372,164 +372,7 @@ static void correct_axis_ceofficient(float ratio) {
* Date : 2020/7/21 logzhan
*---------------------------------------------------------------------**/
void ComputePCA(AHRS_t *ahrs) {
float axis_mean[9];
float freq;
float mold;
int length;
int i;
double attitude[3];
double angle_sum[3] = { 0 };
int angle_len;
//RESET_1时只加ticker，之后直接返回
/* filter bad data */
if (AHRS_STATUS_RESET & ahrs->status) {
++g_tick_p;
return;
}
/* caculate axis ceofficient */
/*PDR_STATUS_CACULATING_AXIS_CEOFFICIENT 0x40*/
if (P_TST_BIT(CACULATING_AXIS_CEOFFICIENT) && \
(g_grav[0]._top - g_grav[0]._bottom + g_grav[0].length + 1) % (g_grav[0].length + 1) > (BUF_LEN >> 1)) {
correct_axis_ceofficient(0.50f);
P_CLR_BIT(CACULATING_AXIS_CEOFFICIENT);
P_SET_BIT(PCA_STABLE);
g_erro._bottom = 0;
for (g_erro._top = 0; g_erro._top < g_erro.length && g_erro._top < 2 * IMU_SAMPLING_FREQUENCY / DOWNSAMPLING_RATE; ++g_erro._top) {
g_erro.data[g_erro._top] = (g_erro._top % 2) ? +0.4f : -0.4f;
}
Buffer_clear((BUFFER *)&g_posi[0]);
Buffer_clear((BUFFER *)&g_posi[1]);
Buffer_clear((BUFFER *)&g_posi[2]);
Buffer_clear((BUFFER *)&g_posi[3]);
g_pdr.bias_direction[0] = 1;
g_pdr.bias_direction[1] = 0;
g_pdr.bias_accuracy = -1;
//++flag;
//}
}
/* correct axis ceofficient */
if ((g_pdr.status & PDR_STATUS_PCA_STABLE) && 0 == (g_tick_p % (2 * IMU_SAMPLING_FREQUENCY))) {
correct_axis_ceofficient(0.10f);
}
/* downsample */
if (0 != (g_tick_p % DOWNSAMPLING_RATE)) {
++g_tick_p;
return;
}
/* push data */
for (i = 0; i < 3; ++i) {
buffer_push((BUFFER *)&g_axis[0 + i], ahrs->x_axis[i]);
buffer_push((BUFFER *)&g_axis[3 + i], ahrs->y_axis[i]);
buffer_push((BUFFER *)&g_axis[6 + i], ahrs->z_axis[i]);
}
for (i = 0; i < 2; ++i) {
//Buffer_push((BUFFER *)&g_grav[i], (float)FILTER_filter(&g_grv_flt[i], ahrs->gravity[i]));
buffer_push((BUFFER *)&g_grav[i], ahrs->gravity[i]);
}
/* motion frequency */
freq = get_main_frequency(g_acc_frq_buf, g_acc_amp_buf, 3);
if (3.5 >= freq && freq > 1.5) {
g_pdr.motionFreq = freq;
}
else if (1.5 >= freq && freq > 0.75){
g_pdr.motionFreq = 2 * freq;
}
/* axis direction */
length = get_optimal_length_by_frequency(g_axis, g_pdr.motionFreq);
get_buffer_mean(&axis_mean[0], &g_axis[0], length);
get_buffer_mean(&axis_mean[3], &g_axis[3], length);
get_buffer_mean(&axis_mean[6], &g_axis[6], length);
memset(g_pdr.axis_direction, 0, sizeof(g_pdr.axis_direction));
for (i = 0; i < 2; ++i) {
g_pdr.axis_direction[i] = g_pdr.axis_ceofficient[0] * axis_mean[0 + i] + g_pdr.axis_ceofficient[1] * axis_mean[3 + i] + g_pdr.axis_ceofficient[2] * axis_mean[6 + i];
}
mold = (float)GET_MOL(g_pdr.axis_direction);
if (0 != mold) {
g_pdr.axis_direction[0] /= mold;
g_pdr.axis_direction[1] /= mold;
}
/* pca direction */
get_pca_direction(g_pdr.pca_direction, g_grav, length);
if (g_pdr.pca_direction[0] * g_pdr.axis_direction[0] + g_pdr.pca_direction[1] * g_pdr.axis_direction[1] < 0) {
g_pdr.pca_direction[0] = -g_pdr.pca_direction[0];
g_pdr.pca_direction[1] = -g_pdr.pca_direction[1];
}
/* direction accuracy */
/*代码疑问1：g_erro为什么由下式确定，什么含义*/
buffer_push((BUFFER *)&g_erro, (float)fabs(g_pdr.axis_direction[0] * g_pdr.pca_direction[1] - g_pdr.axis_direction[1] * g_pdr.pca_direction[0]));
if (!P_TST_BIT(CACULATING_AXIS_CEOFFICIENT)) {
float accuracy;
Buffer_std((BUFFER *)&g_erro, &accuracy);
if (DETECTOR_TYPE_HANDHELD == g_pdr.motionType) {
if (!P_TST_BIT(STABLE) && !P_TST_BIT(CACULATING_AXIS_CEOFFICIENT) && 0.15 >= accuracy) {
P_SET_BIT(CACULATING_AXIS_CEOFFICIENT);
}
else if (g_pdr.pca_accuracy < 0.30 && 0.30 <= accuracy) {
P_CLR_BIT(CACULATING_AXIS_CEOFFICIENT);
P_CLR_BIT(STABLE);
}
}
g_pdr.pca_accuracy = accuracy;
}
else {
g_pdr.pca_accuracy = -1;
}
/* calibrated direction */
g_pdr.cal_direction[0] = g_pdr.pca_direction[0] * g_pdr.bias_direction[0] + g_pdr.pca_direction[1] * g_pdr.bias_direction[1];
g_pdr.cal_direction[1] = g_pdr.pca_direction[1] * g_pdr.bias_direction[0] - g_pdr.pca_direction[0] * g_pdr.bias_direction[1];
//PDR_LOGD("%f %f\n", g_pdr.cal_direction[0],g_pdr.cal_direction[1]);
if (DETECTOR_TYPE_STATIC == g_detector->type || DETECTOR_TYPE_IRREGULAR == g_detector->type) {
g_pdr.motionFreq = 0;
}
Qnb2Att(ahrs->qnb,attitude);
if (angle_count < BUF_LEN - 1)
{
p_angle[0][angle_count] = attitude[0];
p_angle[1][angle_count] = attitude[1];
p_angle[2][angle_count] = attitude[2];
angle_count++;
}
else{
for (int i = 0; i < BUF_LEN - 1; i++)
{
p_angle[0][i] = p_angle[0][i + 1];
p_angle[1][i] = p_angle[1][i + 1];
p_angle[2][i] = p_angle[2][i + 1];
}
p_angle[0][BUF_LEN - 1] = attitude[0];
p_angle[1][BUF_LEN - 1] = attitude[1];
p_angle[2][BUF_LEN - 1] = attitude[2];
angle_len = (length <= BUF_LEN) ? length : BUF_LEN;
for (int j = BUF_LEN - 1; j >(BUF_LEN - 1 - angle_len); j--)
{
angle_sum[0] = angle_sum[0] + p_angle[0][j];
angle_sum[1] = angle_sum[1] + p_angle[1][j];
angle_sum[2] = angle_sum[2] + p_angle[2][j];
}
angle_mean[0] = angle_sum[0] / angle_len;
angle_mean[1] = angle_sum[1] / angle_len;
angle_mean[2] = angle_sum[2] / angle_len;
}
++g_tick_p;
}
/**----------------------------------------------------------------------

3
1.Software/PDR 1.02/src/pdr_detector.c
View File

@ -141,7 +141,7 @@ void pdr_resetDetector(void) {
* Date : 2020/09/02
*
*---------------------------------------------------------------------**/
void updateDetectorImu(imu* imu) {
void UpdateDetectorImu(imu* imu) {
buffer_push((BUFFER *)&g_mol_buf[ACCE], (float)FILTER_filter(&g_mol_flt[ACCE], GET_MOL(imu->acc.s)));
buffer_push((BUFFER *)&g_mol_buf[GYRO], (float)FILTER_filter(&g_mol_flt[GYRO], GET_MOL(imu->gyr.s)));
@ -151,7 +151,6 @@ void updateDetectorImu(imu* imu) {
// ¼ì²âÓû§µÄÔ˶¯ÀàÐÍ
int detectResult = MotionTypeDetect();
//mag_calibration(imu);
if (detectResult == DETECTOR_NO_ERROR && g_detector.lastType != g_detector.type) {
detectorUpdate(&g_detector);
g_detector.lastType = g_detector.type;

81
1.Software/PDR 1.02/src/pdr_kalman.c
View File

@ -42,7 +42,7 @@ static double FUR_duration;
* Description : 初始化卡尔曼滤波器相关
* Date : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
void KalmanFilter_Init(void)
void EKF_Init(void)
{
b_timestamp = -1;
b_last_time = -1;
@ -76,7 +76,7 @@ int clear_buf(double *buf_p, int dmt)
* Description : pdr卡尔曼滤波器的状态预测方程
* Date : 2020/7/22 logzhan
*---------------------------------------------------------------------**/
void pdr_ekfStatePredict(KfPara_t *kf, double stepLen, PDR* g_pdr, int step) {
void EKFStatePredict(KfPara_t *kf, double stepLen, PDR* g_pdr, int step) {
double (*Phi)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[0][0][0]);
double (*pvec1)[N] = (double(*)[N])&(Ekf_Upd_buf_3d[1][0][0]);
@ -349,57 +349,16 @@ int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_
}
/**----------------------------------------------------------------------
* Function : stateRecognition
* Function : StateRecognition
* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳
* Date : 2020/7/22 logzhan
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
void stateRecognition(float *acc, classifer *sys) {
int i,j,k;
float accNorm = 0;
float sum = 0, max_min = 0, max = -1, min = 999;
float mean = 0, std = 0;
void StateRecognition(float *acc, classifer *sys) {
// 先计算当前加速度的模
accNorm = (float)sqrt(vivopow(acc[0], 2) + vivopow(acc[1], 2) + vivopow(acc[2], 2));
// 缓存更新
for (i = 0; i < PATTERN_RECOGNITION_LEN - 1; i++) {
sys->accBuffer[i] = sys->accBuffer[i + 1];
}
sys->accBuffer[PATTERN_RECOGNITION_LEN - 1] = accNorm;
// 计算最大最小值和sum
for (j = 0; j < PATTERN_RECOGNITION_LEN - 1; j++) {
sum += sys->accBuffer[j];
if (sys->accBuffer[j] > max) {
max = sys->accBuffer[j];
}
if (sys->accBuffer[j] < min) {
min = sys->accBuffer[j];
}
}
// 计算均值
mean = sum / PATTERN_RECOGNITION_LEN;
// 计算方差
for (k = 0; k < PATTERN_RECOGNITION_LEN - 1; k++) {
std += (sys->accBuffer[k] - mean) * (sys->accBuffer[k] - mean) / PATTERN_RECOGNITION_LEN;
}
std = (float)sqrt(std);
// 计算最大最小差值
max_min = max - min;
if (std > 5 && max_min > 25) {
sys->type = 1;
}
else {
sys->type = 0;
}
}
double calGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss)
double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss)
{
int i;
double lla[3] = { 0.0 };
@ -536,7 +495,7 @@ double calGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss)
* 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候
* 会造成PDR前后位置不够均匀
*---------------------------------------------------------------------**/
void pdr_insPredict(PDR *g_pdr, StepPredict *stepPredict, imu *ss_data,
void InsPredict(PDR *g_pdr, StepPredict *stepPredict, imu *ss_data,
GNSS_t *pgnss, KfPara_t *kf) {
int step;
long deltaStep;
@ -563,7 +522,7 @@ void pdr_insPredict(PDR *g_pdr, StepPredict *stepPredict, imu *ss_data,
if (lastSteps == 0)deltaStep = 0;
if (g_pdr->heading > 0) {
pdr_ekfStatePredict(kf, stepLength, g_pdr, deltaStep);
EKFStatePredict(kf, stepLength, g_pdr, deltaStep);
g_pdr->fusionPdrFlg = PDR_TRUE;
}
calStepLastTime(stepPredict, ss_data->gyr.time, lastSteps, steps);
@ -572,19 +531,19 @@ void pdr_insPredict(PDR *g_pdr, StepPredict *stepPredict, imu *ss_data,
// 惯导更新位置的目的
int preStep = predictStep(stepPredict, ss_data->gyr.time, lastSteps, pgnss->vel);
if (g_pdr->heading > 0){
pdr_ekfStatePredict(kf, stepLength, g_pdr, preStep);
EKFStatePredict(kf, stepLength, g_pdr, preStep);
if(preStep > 0)g_pdr->fusionPdrFlg = PDR_TRUE;
}
}
}
/**----------------------------------------------------------------------
* Function : resetOutputResult
* Function : ResetOutputResult
* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数
* 2、GPS值赋值给result
* Date : 2020/07/08 logzhan
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
int resetOutputResult(GNSS_t *pgnss, PDR* g_pdr, KfPara_t *kf, lct_fs *result) {
int ResetOutputResult(GNSS_t *pgnss, PDR* g_pdr, KfPara_t *kf, lct_fs *result) {
double stepLength = 0.7;
double ned[3] = { 0 };
@ -592,7 +551,7 @@ int resetOutputResult(GNSS_t *pgnss, PDR* g_pdr, KfPara_t *kf, lct_fs *result) {
g_pdr->pllaInit[0] = pgnss->lat;
g_pdr->pllaInit[1] = pgnss->lon;
g_pdr->pllaInit[2] = 0;
Wgs842Ned(g_pdr->pllaInit, g_pdr->pllaInit, ned);
WGS842NED(g_pdr->pllaInit, g_pdr->pllaInit, ned);
g_pdr->x0 = ned[0];
g_pdr->y0 = ned[1];
@ -632,7 +591,7 @@ int pdr_initProc(GNSS_t *pgnss, KfPara_t *kf, PDR* g_pdr,lct_fs *result)
// plla 转 ned
double ned[3] = { 0 };
Wgs842Ned(g_pdr->pllaInit, g_pdr->pllaInit, ned);
WGS842NED(g_pdr->pllaInit, g_pdr->pllaInit, ned);
// 在NED坐标系的计算, 单位是米
g_pdr->x0 = ned[0];
@ -669,13 +628,13 @@ int pdr_initProc(GNSS_t *pgnss, KfPara_t *kf, PDR* g_pdr,lct_fs *result)
}
/**----------------------------------------------------------------------
* Function : CalEKFQRMatrix
* Function : EKFCalQRMatrix
* Description : 根据GPS信号特征调整卡尔曼滤波噪声矩阵，q矩阵是过程噪声矩阵，
* 需要跟惯导预测位置精度相关。r矩阵为GNSS观测噪声，跟GPS输出的
* 信息精度有关。
* Date : 2020/07/09 logzhan
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
void CalEKFQRMatrix(lct_nmea *nmea, PDR *g_pdr, classifer *sys,
void EKFCalQRMatrix(lct_nmea *nmea, PDR *g_pdr, classifer *sys,
KfPara_t *kf) {
if (g_pdr->motionType != PDR_MOTION_TYPE_HANDHELD &&
@ -722,7 +681,7 @@ void CalEKFQRMatrix(lct_nmea *nmea, PDR *g_pdr, classifer *sys,
* Description : PDR的GNSS和INS融合定位
* Date : 2020/07/09 logzhan
*---------------------------------------------------------------------**/
void pdr_gnssInsLocFusion(GNSS_t *pgnss, PDR *g_pdr, classifer *sys, double yaw_bias,
void GnssInsLocFusion(GNSS_t *pgnss, PDR *g_pdr, classifer *sys, double yaw_bias,
KfPara_t *kf, lct_fs *res) {
double plla[3] = { 0 };
double ned[3] = { 0 };
@ -734,7 +693,7 @@ void pdr_gnssInsLocFusion(GNSS_t *pgnss, PDR *g_pdr, classifer *sys, double yaw_
plla[1] = pgnss->lon;
plla[2] = 0;
Wgs842Ned(plla, g_pdr->pllaInit, ned);
WGS842NED(plla, g_pdr->pllaInit, ned);
pgnss->xNed = ned[0];
pgnss->yNed = ned[1];
@ -753,7 +712,7 @@ void pdr_gnssInsLocFusion(GNSS_t *pgnss, PDR *g_pdr, classifer *sys, double yaw_
ned[1] = kf -> p_xk[1] - g_pdr->y0;
ned[2] = 0;
ned2Wgs84(g_pdr->pllaInit, ned, plla);
NED2WGS84(g_pdr->pllaInit, ned, plla);
res->latitude = plla[0];
res->latitude_ns = pgnss->lat_ns;

59
1.Software/PDR 1.02/src/pdr_location.c
View File

@ -61,7 +61,7 @@ void NavSys_Init(void) {
// PDR初始化
pdr = Base_Init();
//base文件中姿态角计算初始化
KalmanFilter_Init();
EKF_Init();
// 场景识别模块初始化
initSceneRecognition();
@ -99,26 +99,26 @@ void ResetSystemStatus(KfPara_t *kf)
* Description : PDR 惯性导航定位
* Date : 2022/09/16 logzhan
*---------------------------------------------------------------------**/
int InsLocation(imu* ss_data, KfPara_t* kf) {
int InsLocation(imu* imuData, KfPara_t* kf) {
// AHRS姿态更新占2%的运行时间
if (UpdateAHRS(ss_data)) {
if (UpdateAHRS(imuData)) {
// 完成姿态估计后计算PCA， 占26%的运行时间
//ComputePCA(&g_ahrs);
ComputePCA(&g_ahrs);
}
// 更新用户运动类型检测器IMU信息以及用户运动类型检测, 占21%的运行时间
updateDetectorImu(ss_data);
UpdateDetectorImu(imuData);
pdr->gyroTime = ss_data->gyr.time;
pdr->gyroTime = imuData->gyr.time;
// 为什么手持类型是0.1的运动频率？
if (pdr->motionType == DETECTOR_TYPE_HANDHELD)pdr->motionFreq = 0.1f;
// 计步器输入传感器数据，占10%的运行时间
updatePedometer(ss_data, &pdr->steps);
UpdatePedometer(imuData, &pdr->steps);
// ----- Filter Initialization ------- //
if (pdr->sysStatus == IS_INITIAL) {
pdr->ts = ss_data->gyr.time;
pdr->ts = imuData->gyr.time;
pdr->lastSteps = pdr->steps;
stepPredict.lastTime = 0;
stepPredict.fnum = 0;
@ -126,30 +126,17 @@ int InsLocation(imu* ss_data, KfPara_t* kf) {
return 0;
}
pdr->ts = ss_data->gyr.time;
pdr->ts = imuData->gyr.time;
if ((0 == ss_data->acc.s[0] && 0 == ss_data->acc.s[1] && 0 == ss_data->acc.s[2]) ||
(0 == ss_data->mag.s[0] && 0 == ss_data->mag.s[1] && 0 == ss_data->mag.s[2])) {
if ((0 == imuData->acc.s[0] && 0 == imuData->acc.s[1] && 0 == imuData->acc.s[2]) ||
(0 == imuData->mag.s[0] && 0 == imuData->mag.s[1] && 0 == imuData->mag.s[2])) {
return 0;
}
// 识别当前状态是否平稳，这个函数计算量到达整个系统10%的计算量, 以后根据情况进行优化
// stateRecognition(ss_data->acc.s, &sys);
StateRecognition(imuData->acc.s, &sys);
//pdr->heading = calPredAngle(pdr, &sys, pdr->gpsHeading, pdr->trackHeading, ss_data);
//if (yaw_bias != -1) {
// if (pdr->pca_accuracy > 0 && pdr->pca_accuracy < 0.1 && pdr->motionType == 3 &&
// (pdr->bias_accuracy > 1.5 || pdr->bias_accuracy == -1 || pgnss.error > 30)
// && (fabs(yaw_bias - 2 * PI) < YAW_THRES * PI / 180 || fabs(yaw_bias - PI) < YAW_THRES * PI / 180 ||
// fabs(yaw_bias) < YAW_THRES * PI / 180)) {
// dir[0] = pdr->pca_direction[0] * bias_dir[0] + pdr->pca_direction[1] * bias_dir[1];
// dir[1] = pdr->pca_direction[1] * bias_dir[0] - pdr->pca_direction[0] * bias_dir[1];
// pdr->heading = atan2(dir[1], dir[0]);
// if (pdr->heading < 0) {
// pdr->heading += TWO_PI;
// }
// }
//}
//pdr->heading = CalPredHeading(...);
double imuYaw = - g_ahrs.yaw;
if (imuYaw < 0) {
@ -163,7 +150,7 @@ int InsLocation(imu* ss_data, KfPara_t* kf) {
}
// PDR 惯性位置预测
pdr_insPredict(pdr, &stepPredict, ss_data, &pgnss, kf);
InsPredict(pdr, &stepPredict, imuData, &pgnss, kf);
pdr->lastSteps = pdr->steps;
pdr->lastHeading = pdr->heading;
@ -200,26 +187,26 @@ int GnssInsFusionLocation(lct_nmea *nmea_data, KfPara_t *kf, lct_fs *result)
result->motionType = pdr->motionType;
#endif
calPdrHeadingOffset(nmea_data, pdr);
CalPdrHeadingOffset(nmea_data, pdr);
// 计算两次GPS时间之间，步数变化量
pdr->deltaStepsPerGps = pdr->steps - pdr->lastGpsSteps;
pdr->lastGpsSteps = pdr->steps;
int mode = pdr_detectFixMode(&pgnss, kf, pdr, result);
int mode = DetectFixMode(&pgnss, kf, pdr, result);
if (mode != TYPE_FIX_PDR)return mode;
//根据历史GPS拟合输出预估的GPS轨迹航向角
pdr->trackHeading = calGnssTrackHeading(GpsPos, pgnss);
pdr->trackHeading = CalGnssTrackHeading(GpsPos, pgnss);
// 如果系统没有初始化，则先初始化
if (pdr->sysStatus == IS_INITIAL) {
return pdr_initProc(&pgnss, kf, pdr, result);
}
// 如果惯导没有达到输出条件，则不预测轨迹
if (!pdr->fusionPdrFlg)return resetOutputResult(&pgnss, pdr, kf, result);
if (!pdr->fusionPdrFlg)return ResetOutputResult(&pgnss, pdr, kf, result);
// 根据GPS信息, 计算kf的过程噪声和观测噪声
CalEKFQRMatrix(nmea_data, pdr, &sys, kf);
EKFCalQRMatrix(nmea_data, pdr, &sys, kf);
if (pgnss.satellites_num > 4){
// 如果GPS航向角处于无效状态且速度也是无效状态
@ -232,7 +219,7 @@ int GnssInsFusionLocation(lct_nmea *nmea_data, KfPara_t *kf, lct_fs *result)
pgnss.yaw = (float)kf->p_xk[3];
}
if (pgnss.yaw != INVAILD_GPS_YAW) {
pdr_gnssInsLocFusion(&pgnss, pdr, &sys, yaw_bias, kf, result);
GnssInsLocFusion(&pgnss, pdr, &sys, yaw_bias, kf, result);
return TYPE_FIX_PDR;
}
}
@ -267,7 +254,7 @@ void NoGnssInfoPredict(KfPara_t* kf, lct_fs* result, PDR* p_pdr)
ned[1] = kf->p_xk[1] - p_pdr->y0;
ned[2] = 0;
ned2Wgs84(p_pdr->pllaInit, ned, lla);
NED2WGS84(p_pdr->pllaInit, ned, lla);
result->latitude = lla[0];
result->latitude_ns = pgnss.lat_ns;
@ -282,7 +269,7 @@ void NoGnssInfoPredict(KfPara_t* kf, lct_fs* result, PDR* p_pdr)
* Description : 利用GPS信号较好时的偏航角修正磁力计计算方向键的偏移
* Date : 2020/07/08 logzhan
*---------------------------------------------------------------------**/
void calPdrHeadingOffset(lct_nmea* nmea_data, PDR* p_pdr) {
void CalPdrHeadingOffset(lct_nmea* nmea_data, PDR* p_pdr) {
//惯导姿态角与GPS行进角度的对准（判断GPS信号是否正常以及惯导是否平稳）
if (refGpsYaw != -1000)return;
@ -312,7 +299,7 @@ void calPdrHeadingOffset(lct_nmea* nmea_data, PDR* p_pdr) {
* Date : 2020/07/08 logzhan : 修改-1为INVAILD_GPS_YAW，提高
* 代码的可读性
*---------------------------------------------------------------------**/
int pdr_detectFixMode(GNSS_t* pgnss, KfPara_t* kf, PDR* g_pdr, lct_fs* result) {
int DetectFixMode(GNSS_t* pgnss, KfPara_t* kf, PDR* g_pdr, lct_fs* result) {
// 检测用户是否处于静止状态
if (pdr_detStatic(pdr, pgnss, g_pdr->deltaStepsPerGps)) {
if (pgnss->error > 0 && pgnss->error < 15 &&

11
1.Software/PDR 1.02/src/pdr_main.cpp
View File

@ -79,7 +79,8 @@ extern "C" double refGpsYaw;
*---------------------------------------------------------------------**/
int main(int argc, char* argv[])
{
string logPath = "E:\\Software Project\\GnssIns-PDR-Private\\1.Software - 2.0\\Debug\\";
string logPath = "E:\\SoftwareProject\\GnssIns\\GnssIns-PDR-Private\\1.Software\\PDR 1.02\\Debug\\";
string logDate = "shenzhen";
string logLabel = "data\\";
string fileHead;
@ -122,7 +123,7 @@ int main(int argc, char* argv[])
// 解析文本数据仿真
if (ParseLineAndUpdate(line, &locFusion) != TYPE_FIX_NONE){
// 更新结果轨迹，用于最后的kml生成
updateResTrack(resTracks, locFusion);
UpdateResTrack(resTracks, locFusion);
}
}
// 关闭PDR算法，释放资源
@ -185,11 +186,11 @@ double gpsYaw2GoogleYaw(double heading) {
}
/**----------------------------------------------------------------------
* Function : updateResTrack
* Function : UpdateResTrack
* Description : 更新新输出的结果轨迹，包含GPS轨迹和PDR轨迹
* Date : 2021/01/25 logzhan
* Date : 2022/09/19 logzhan
*---------------------------------------------------------------------**/
void updateResTrack(ResultTracks& resTrack, lct_fs& lctfs)
void UpdateResTrack(ResultTracks& resTrack, lct_fs& lctfs)
{
resTrack.pdrTrack[resTrack.pdrLen].lat = lctfs.latitude;
resTrack.pdrTrack[resTrack.pdrLen].lon = lctfs.longitudinal;

8
1.Software/PDR 1.02/src/pdr_util.c
View File

@ -317,7 +317,7 @@ void NED2ECEF(double *plla, double *ned, double *ecef0, double *ecef) {
ecef[2] = cos(plla[0])*ned[0] - sin(plla[0])*ned[2] + ecef0[2];;
}
void Wgs842Ned(double *plla, double *ref_lla, double *ned)
void WGS842NED(double *plla, double *ref_lla, double *ned)
{
double ecef[3] = {0};
@ -325,7 +325,7 @@ void Wgs842Ned(double *plla, double *ref_lla, double *ned)
ECEF2Ned(ecef, ref_lla, ned);
}
void ned2Wgs84(double *ref_plla, double *ned, double* plla)
void NED2WGS84(double *ref_plla, double *ned, double* plla)
{
double ecef[3] = { 0 };
double ref_ecef[3] = { 0 };
@ -477,8 +477,8 @@ double CalDistance(LatLng pointA, LatLng pointB)
if (isRealLat(pointA.lat) && isRealLon(pointA.lon)
&& isRealLat(pointB.lat) && isRealLon(pointB.lon))
{
Wgs842Ned(lla1, lla1, ned1);
Wgs842Ned(lla2, lla1, ned2);
WGS842NED(lla1, lla1, ned1);
WGS842NED(lla2, lla1, ned2);
diff_ned[0] = ned1[0] - ned2[0];
diff_ned[1] = ned1[1] - ned2[1];
diff_ned[2] = ned1[2] - ned2[2];

2
1.Software/PDR 1.02/src/steps.c
View File

@ -128,7 +128,7 @@ void reinitPedometer(void)
* Description : PDR 计步器的加速度输入, 并通过指针返回用户步数
* Date : 2020/2/1 logzhan
*---------------------------------------------------------------------**/
void updatePedometer(imu* ss_data, unsigned long* step)
void UpdatePedometer(imu* ss_data, unsigned long* step)
{
unsigned char k = 0;