167 lines
5.2 KiB
C
167 lines
5.2 KiB
C
|
#ifndef _PDR_UTIL_H_
|
|||
|
|
#define _PDR_UTIL_H_
|
|||
|
|
|
|||
|
|
#ifdef __cplusplus
|
|||
|
|
extern "C" {
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#include <stdio.h>
|
|||
|
|
|
|||
|
|
|
|||
|
|
#define TRAJ_POINT_COUNT 1000
|
|||
|
|
#define MIN_POINT_COUNT 15
|
|||
|
|
#define OPT_TRAJ_TIMEOUT 1000
|
|||
|
|
|
|||
|
|
#define FALIED_LINE 0
|
|||
|
|
#define IN_LINE 1
|
|||
|
|
#define CLOSE_START 2
|
|||
|
|
#define CLOSE_END 3
|
|||
|
|
|
|||
|
|
typedef struct LatLng{
|
|||
|
|
double lat;
|
|||
|
|
double lon;
|
|||
|
|
} LatLng;
|
|||
|
|
|
|||
|
|
typedef struct TrajSign
|
|||
|
|
{
|
|||
|
|
int count;
|
|||
|
|
double lastTime;
|
|||
|
|
double weight;
|
|||
|
|
double yaw[TRAJ_POINT_COUNT];
|
|||
|
|
double error[TRAJ_POINT_COUNT];
|
|||
|
|
double points[TRAJ_POINT_COUNT][2];
|
|||
|
|
}TrajSign;
|
|||
|
|
|
|||
|
|
/**地球相关参数*******************************************************************/
|
|||
|
|
typedef struct
|
|||
|
|
{
|
|||
|
|
double rmh; /* 子午圈曲率半径 */
|
|||
|
|
double rnh; /* 卯酉圈曲率半径 */
|
|||
|
|
double grav; /* 当地重力加速度 */
|
|||
|
|
double lat; /* 当地纬度（rad）*/
|
|||
|
|
double wnie[3]; /* 地理系相对惯性系的角速度在导航系分量 */
|
|||
|
|
} EarthData_t;
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : mean
|
|||
|
|
* Description : 对输入double数组求均值
|
|||
|
|
* Review : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用
|
|||
|
|
* 不同函数
|
|||
|
|
* Date : 2020/7/4 logzhan
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
double mean(double *x, int n);
|
|||
|
|
|
|||
|
|
float fmean(float *data, int len);
|
|||
|
|
|
|||
|
|
float stdf(float* data, int len);
|
|||
|
|
|
|||
|
|
double meanAngle(double* angle, int len);
|
|||
|
|
|
|||
|
|
void modAngle(double * angle, double min, double max);
|
|||
|
|
|
|||
|
|
double pow_i(double num, long long n);
|
|||
|
|
|
|||
|
|
double pow_f(double num, double m);
|
|||
|
|
|
|||
|
|
double vivopow(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);
|
|||
|
|
|
|||
|
|
int Jacobi(double a[][2], double p[][2], int n, double eps, int T);
|
|||
|
|
|
|||
|
|
void QuaternConj(float _q[], float q[]);
|
|||
|
|
|
|||
|
|
void quaternProd(float ab[], float a[], float b[]);
|
|||
|
|
|
|||
|
|
void WGS842ECEF(double *plla, double *ecef);
|
|||
|
|
|
|||
|
|
void ECEF2WGS84(double *ecef, double *plla);
|
|||
|
|
|
|||
|
|
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 ned2Wgs84(double *ref_plla, double *ned, double* plla);
|
|||
|
|
|
|||
|
|
|
|||
|
|
LatLng ProjPointOfLatLng(LatLng point, LatLng linePointA, LatLng linePointB);
|
|||
|
|
|
|||
|
|
double CalDistance(LatLng pointA, LatLng pointB);
|
|||
|
|
|
|||
|
|
void ProjPointOfLatLng_cir(double point1[], double yaw, double point2[], double result[]);
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : pdr_invSqrt
|
|||
|
|
* Description : 计算1/sqrt(x)的快速的算法
|
|||
|
|
* Date : 2020/6/30 logzhan
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
float InvSqrt(float x);
|
|||
|
|
|
|||
|
|
/**---------------------------------------------------------------------
|
|||
|
|
* Function : pdr_v3Norm
|
|||
|
|
* Description : 三维浮点数向量归一化
|
|||
|
|
* Date : 2021/01/26 yuanlin@vivocom &&
|
|||
|
|
*
|
|||
|
|
*
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
void pdr_v3Norm(float* vx, float* vy, float* vz);
|
|||
|
|
|
|||
|
|
/**---------------------------------------------------------------------
|
|||
|
|
* Function : quatNorm
|
|||
|
|
* Description : 四元数归一化
|
|||
|
|
* Date : 2021/01/26 yuanlin@vivocom && logzhan
|
|||
|
|
* 2021/01/27 logzhan : 优化了代码逻辑，防止当四元数
|
|||
|
|
* 全为0时产生的异常情况。
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
void QuatNorm(float* q0, float* q1, float* q2, float* q3);
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : pdr_writeCsvStr
|
|||
|
|
* Description : 将字符串信息写入文本，主要是对fprintf封装一层
|
|||
|
|
* Date : 2020/7/8 logzhan
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
void WriteCsvStr(FILE* fp_write, char* str);
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : pdr_utc2hms
|
|||
|
|
* Description : 将UTC时间转换为时分秒
|
|||
|
|
* Date : 2020/7/8 logzhan
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
void pdr_utc2hms(double utc, double* h, double* m, double* s);
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : qnb2att
|
|||
|
|
* Description : 四元数转欧拉角
|
|||
|
|
* Date : 2020/7/4 logzhan
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
void Qnb2Att(float* q, double* attitude);
|
|||
|
|
|
|||
|
|
double CalRadianDifferent(double s_dir, double d_dir);
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : pdr_earthParameter
|
|||
|
|
* Description : 根据输入纬度返回该纬度地球相关参数
|
|||
|
|
* Date : 2020/7/8 logzhan
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
EarthData_t CalEarthParameter(double oriLat);
|
|||
|
|
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : Motion2TypeStr
|
|||
|
|
* Description : 把用户运动类型转换为字符串输出
|
|||
|
|
* Date : 2022/9/16 logzhan
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
const char* Motion2TypeStr(int type);
|
|||
|
|
|
|||
|
|
int pdr_min(int param_a, int param_b);
|
|||
|
|
|
|||
|
|
#ifdef __cplusplus
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|