466 lines
13 KiB
C
466 lines
13 KiB
C
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||
* File Name : pdr_util.c
|
||
* Department : Sensor Algorithm Team
|
||
* Current Version : V1.2
|
||
* Author : logzhan
|
||
* Date of Issued : 2020.7.4
|
||
* Comments : PDR 工具类函数支持
|
||
********************************************************************************/
|
||
/* Header File Including -----------------------------------------------------*/
|
||
#include <stdio.h>
|
||
#include <math.h>
|
||
#include <float.h>
|
||
#include "PDRBase.h"
|
||
#include "Utils.h"
|
||
#include "CoordTrans.h"
|
||
|
||
const float FLT_THRES = 0.000001f; // 浮点数最小阈值
|
||
|
||
/* Function Declaration ------------------------------------------------------*/
|
||
/**----------------------------------------------------------------------
|
||
* Function : mean
|
||
* Description : 对输入double数组求均值
|
||
* Review : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用
|
||
* 不同函数
|
||
* Date : 2020/7/4 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
double mean(double *data, int len) {
|
||
int i;
|
||
double value = 0;
|
||
for (i = 0; i < len; i++) {
|
||
value += data[i] / len;
|
||
}
|
||
return value;
|
||
}
|
||
/**----------------------------------------------------------------------
|
||
* Function : fmean
|
||
* Description : 对输入float数组求均值
|
||
* Review : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用
|
||
* 不同函数
|
||
* Date : 2020/7/4 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
float fmean(float *data, int len)
|
||
{
|
||
float value = 0;
|
||
for (int i = 0; i < len; i++){
|
||
value += data[i] / len;
|
||
}
|
||
return value;
|
||
}
|
||
|
||
double meanAngle(double* angle, int len)
|
||
{
|
||
double baseAngle = angle[0];
|
||
double diff = 0.0;
|
||
double sum = 0.0;
|
||
double value = 0;
|
||
|
||
if (len <= 0)return 0;
|
||
|
||
for (int i = 0; i < len; i++)
|
||
{
|
||
diff = angle[i] - baseAngle;
|
||
if (diff > PI)
|
||
{
|
||
diff = diff - TWO_PI;
|
||
}
|
||
else if (diff < -PI)
|
||
{
|
||
diff = diff + TWO_PI;
|
||
}
|
||
|
||
sum += (baseAngle + diff);
|
||
}
|
||
|
||
value = sum / len;
|
||
|
||
modAngle(&value, 0, TWO_PI);
|
||
return value;
|
||
}
|
||
|
||
void modAngle(double * angle, double min, double max)
|
||
{
|
||
double value = *angle;
|
||
double range = max - min;
|
||
while (value > max)value = value - range;
|
||
while (value < min)value = value + range;
|
||
*angle = value;
|
||
}
|
||
|
||
float stdf(float* data, int len)
|
||
{
|
||
int i;
|
||
float meanValue = fmean(data, len);
|
||
float value = 0;
|
||
for (i = 0; i < len; i++)
|
||
{
|
||
value += (data[i] - meanValue) * (data[i] - meanValue) / len;
|
||
}
|
||
return (float)sqrt(value);
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : pdr_invSqrt
|
||
* Description : 计算1/sqrt(x)的快速的算法
|
||
* Date : 2020/6/30 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
float InvSqrt(float x) {
|
||
float xHalf = 0.5f * x;
|
||
int i = *(int *)&x;
|
||
i = 0x5f3759df - (i >> 1);
|
||
x = *(float *)&i;
|
||
x = x * (1.5f - xHalf * x * x);
|
||
return x;
|
||
}
|
||
|
||
/**---------------------------------------------------------------------
|
||
* Function : Vec3Norm
|
||
* Description : 三维浮点数向量归一化
|
||
* Date : 2022/11/1 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void Vec3Norm(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;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : Motion2TypeStr
|
||
* Description : 把用户运动类型转换为字符串输出
|
||
* Date : 2022/9/16 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
const char* Motion2TypeStr(int type)
|
||
{
|
||
if (type == PDR_MOTION_TYPE_STATIC) {
|
||
return "STATIC";
|
||
}
|
||
else if (type == PDR_MOTION_TYPE_IRREGULAR) {
|
||
return "IRREGULAR";
|
||
}
|
||
else if (type == PDR_MOTION_TYPE_HANDHELD) {
|
||
return "HANDLED";
|
||
}
|
||
else if (type == PDR_MOTION_TYPE_SWINGING) {
|
||
return "SWINGING";
|
||
}
|
||
return "UNKOWN";
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : qnb2att
|
||
* Description : 四元数转欧拉角
|
||
* Date : 2020/7/4 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void Qnb2Att(float* q, double* attitude)
|
||
{
|
||
double q11 = q[0] * q[0]; double q13 = q[0] * q[2];
|
||
double q12 = q[0] * q[1]; double q14 = q[0] * q[3];
|
||
double q22 = q[1] * q[1]; double q24 = q[1] * q[3];
|
||
double q23 = q[1] * q[2]; double q33 = q[2] * q[2];
|
||
double q34 = q[2] * q[3]; double q44 = q[3] * q[3];
|
||
|
||
if (fabs(q34 + q12) <= 0.5) {
|
||
attitude[0] = asin(2 * (q34 + q12));
|
||
}
|
||
attitude[1] = atan2(-2 * (q24 - q13), q11 - q22 - q33 + q44);
|
||
attitude[2] = atan2(-2 * (q23 - q14), q11 - q22 + q33 - q44);
|
||
}
|
||
|
||
|
||
|
||
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;
|
||
m_x = mean(x, n);
|
||
m_y = mean(y, n);
|
||
for (i = 0; i < n; i++) {
|
||
sum_xy += (x[i] - m_x)*(y[i] - m_y);
|
||
sum_x += (x[i] - m_x)*(x[i] - m_x);
|
||
sum_y += (y[i] - m_y)*(y[i] - m_y);
|
||
}
|
||
cov[0][0] = sum_x / n;
|
||
cov[0][1] = sum_xy / n;
|
||
cov[1][0] = sum_xy / n;
|
||
cov[1][1] = sum_y / n;
|
||
}
|
||
|
||
int Jacobi(double a[][2], double p[][2], int n, double eps, int T)
|
||
{
|
||
int i, j, k;
|
||
double max = a[0][1];
|
||
int row = 0;
|
||
int col = 0;
|
||
int ite_num = 0;
|
||
double theta = 0;
|
||
double aii = 0;
|
||
double ajj = 0;
|
||
double aij = 0;
|
||
double sin_theta = 0;
|
||
double cos_theta = 0;
|
||
double sin_2theta = 0;
|
||
double cos_2theta = 0;
|
||
double arowk = 0;
|
||
double acolk = 0;
|
||
double pki = 0;
|
||
double pkj = 0;
|
||
|
||
for (i = 0; i < n; i++)
|
||
p[i][i] = 1;
|
||
while (1)
|
||
{
|
||
max = fabs(a[0][1]);
|
||
row = 0;
|
||
col = 1;
|
||
for (i = 0; i < n; i++)
|
||
for (j = 0; j < n; j++)
|
||
if (i != j && fabs(a[i][j])>max)
|
||
{
|
||
max = fabs(a[i][j]);
|
||
row = i;
|
||
col = j;
|
||
}
|
||
if (max < eps)
|
||
{
|
||
//printf("max : %lf \n", max);
|
||
//printf("t : %lf \n", ite_num);
|
||
|
||
return 1;
|
||
}
|
||
|
||
if (ite_num>T)
|
||
{
|
||
//printf("max : %lf \n", max);
|
||
//printf("t : %lf \n", ite_num);
|
||
return 0;
|
||
}
|
||
|
||
if (a[row][row] == a[col][col])
|
||
theta = PI / 4;
|
||
else
|
||
theta = 0.5*atan(2 * a[row][col] / (a[row][row] - a[col][col]));
|
||
aii = a[row][row];
|
||
ajj = a[col][col];
|
||
aij = a[row][col];
|
||
sin_theta = sin(theta);
|
||
cos_theta = cos(theta);
|
||
sin_2theta = sin(2 * theta);
|
||
cos_2theta = cos(2 * theta);
|
||
a[row][row] = aii * cos_theta*cos_theta + ajj * sin_theta*sin_theta + aij * sin_2theta;
|
||
a[col][col] = aii * sin_theta*sin_theta + ajj * cos_theta*cos_theta - aij * sin_2theta;
|
||
a[row][col] = 0.5*(ajj - aii)*sin_2theta + aij * cos_2theta;
|
||
a[col][row] = a[row][col];
|
||
for (k = 0; k < n; k++)
|
||
{
|
||
if (k != row && k != col)
|
||
{
|
||
arowk = a[row][k];
|
||
acolk = a[col][k];
|
||
a[row][k] = arowk * cos_theta + acolk * sin_theta;
|
||
a[k][row] = a[row][k];
|
||
a[col][k] = acolk * cos_theta - arowk * sin_theta;
|
||
a[k][col] = a[col][k];
|
||
}
|
||
}
|
||
if (ite_num == 0)
|
||
{
|
||
p[row][row] = cos_theta;
|
||
p[row][col] = -sin_theta;
|
||
p[col][row] = sin_theta;
|
||
p[col][col] = cos_theta;
|
||
}
|
||
else
|
||
{
|
||
for (k = 0; k < n; k++)
|
||
{
|
||
pki = p[k][row];
|
||
pkj = p[k][col];
|
||
p[k][row] = pki * cos_theta + pkj * sin_theta;
|
||
p[k][col] = pkj * cos_theta - pki * sin_theta;
|
||
}
|
||
}
|
||
ite_num++;
|
||
}
|
||
}
|
||
|
||
static int isRealLat(double lat){ return (lat < 90) && (lat > -90); }
|
||
static int isRealLon(double lon){ return (lon < 180) && (lon > -180); }
|
||
|
||
/*
|
||
function: 计算地图上两个由经纬度表示点间的直线距离。
|
||
param @ pointA : 点A的经纬度
|
||
param @ pointB : 点B的经纬度
|
||
return @ dis : A、B点间的距离；若小于0则说明A或B点的经纬度输入有误。
|
||
*/
|
||
double CalDistance(LatLng_t pointA, LatLng_t pointB)
|
||
{
|
||
double dis = -1;
|
||
double lla1[3] = { 0 };
|
||
double lla2[3] = { 0 };
|
||
double ned1[3] = { 0 };
|
||
double ned2[3] = { 0 };
|
||
double diff_ned[3] = { 0 };
|
||
|
||
lla1[0] = pointA.lat * RADIAN_PER_DEG;
|
||
lla1[1] = pointA.lon * RADIAN_PER_DEG;
|
||
lla2[0] = pointB.lat * RADIAN_PER_DEG;
|
||
lla2[1] = pointB.lon * RADIAN_PER_DEG;
|
||
|
||
if (isRealLat(pointA.lat) && isRealLon(pointA.lon)
|
||
&& isRealLat(pointB.lat) && isRealLon(pointB.lon))
|
||
{
|
||
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];
|
||
|
||
dis = sqrt(pow(diff_ned[0], 2) + pow(diff_ned[1], 2) + pow(diff_ned[2], 2));
|
||
}
|
||
return dis;
|
||
}
|
||
|
||
|
||
|
||
LatLng_t ProjPointOfLatLng(LatLng_t point, LatLng_t linePointA, LatLng_t linePointB)
|
||
{
|
||
|
||
/* local variables declaration */
|
||
double gradxB_P; /* linePointB 向 point 的 x 梯度 */
|
||
double gradxB_A; /* linePointA 向 linePointB 的 x 梯度 */
|
||
double gradyB_A; /* linePointA 向 linePointB 的 y 梯度 */
|
||
double normPow; /* linePointA 向 linePointB 梯度的平方和 */
|
||
double scalarMultip; /* gradxB_A * gradyB_A 的标量乘法 */
|
||
LatLng_t resProjPoint; /* 结果点坐标 */
|
||
|
||
/* block process */
|
||
gradxB_A = linePointA.lat - linePointB.lat;
|
||
gradyB_A = linePointA.lon - linePointB.lon;
|
||
if ((fabs(gradxB_A) < DBL_EPSILON) && (fabs(gradyB_A) < DBL_EPSILON))
|
||
{
|
||
/* 当linePointA 和linePointB是同一点时，返回的point坐标 */
|
||
resProjPoint.lat = point.lat;
|
||
resProjPoint.lon = point.lon;
|
||
}
|
||
else
|
||
{
|
||
gradxB_P = point.lat - linePointB.lat;
|
||
normPow = pow(gradxB_A, 2) + pow(gradyB_A, 2);
|
||
resProjPoint.lon = (double)(
|
||
(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轴平行时做规避*/
|
||
scalarMultip = (fabs(gradxB_A) < DBL_EPSILON) ? gradyB_A : gradxB_A;
|
||
}
|
||
else
|
||
{
|
||
scalarMultip = gradxB_A * gradyB_A;
|
||
}
|
||
resProjPoint.lat = (double)(
|
||
(gradxB_A * linePointB.lat * gradyB_A + gradxB_A * gradxB_A * (resProjPoint.lon - linePointB.lon)) / scalarMultip);
|
||
if (fabs(gradxB_A) < DBL_EPSILON)
|
||
{
|
||
resProjPoint.lat = linePointA.lat;
|
||
}
|
||
if (fabs(gradyB_A) < DBL_EPSILON)
|
||
{
|
||
resProjPoint.lon = point.lon;
|
||
}
|
||
}
|
||
|
||
return resProjPoint;
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : CalEarthParameter
|
||
* Description : 根据输入纬度返回该纬度地球相关参数
|
||
* Date : 2020/7/8 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
EarthData_t CalEarthParameter(double lat)
|
||
{
|
||
double temp_value = 0;
|
||
EarthData_t earthData = { 0 };
|
||
|
||
if (fabs(lat) >= PI / 2)lat = 0.0f;
|
||
|
||
temp_value = sqrt(1 - WGS84_ECCENTR2 * (sin(lat)*sin(lat)));
|
||
|
||
earthData.lat = lat;
|
||
earthData.rmh = WGS84_RE * (1 - WGS84_ECCENTR2) / (temp_value*temp_value*temp_value);
|
||
earthData.rnh = WGS84_RE * cos(lat) / temp_value;
|
||
return earthData;
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : pdr_CalRadianDifferent
|
||
* Description : 计算弧度之差
|
||
* Date : 2020/7/8 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
double CalRadianDifferent(double s_dir, double d_dir)
|
||
{
|
||
double dirDiff = d_dir - s_dir;
|
||
if (fabs(dirDiff) < PI)return dirDiff;
|
||
return dirDiff > 0 ? dirDiff - 2 * PI : dirDiff + 2 * PI;
|
||
}
|
||
|
||
void ProjPointOfLatLng_cir(double point1[], double yaw, double point2[], double result[])
|
||
{
|
||
if (fabs(fabs(yaw) - PI / 2) < DOUBLE_ZERO)
|
||
{
|
||
result[0] = point1[0];
|
||
result[1] = point2[1];
|
||
return;
|
||
}
|
||
|
||
EarthData_t earthData = CalEarthParameter(point1[0]);
|
||
double k = -tan(yaw);
|
||
double lonDiff = (point2[1] - point1[1]) * earthData.rnh;
|
||
double latDiff = (point2[0] - point1[0]) * earthData.rmh;
|
||
|
||
double x = (k*latDiff + k * k * lonDiff) / (1 + k * k);
|
||
double y = (latDiff + k * lonDiff) / (1 + k * k);
|
||
|
||
result[1] = point1[1] + x / earthData.rnh;
|
||
result[0] = point1[0] + y / earthData.rmh;
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : pdr_WriteCsvStr
|
||
* Description : 将字符串信息写入文本，主要是对fprintf封装一层
|
||
* Date : 2020/7/8 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void WriteCsvStr(FILE* fp_write, char* str)
|
||
{
|
||
if (NULL == fp_write)return;
|
||
fprintf(fp_write, "%s\n", str);
|
||
}
|
||
|
||
int pdr_min(int a, int b){
|
||
return (a > b ? b : a);
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : pdr_utc2hms
|
||
* Description : 将UTC时间转换为时分秒
|
||
* Date : 2020/7/8 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void pdr_utc2hms(double utc, double* h, double* m, double* s) {
|
||
|
||
long lutc = (long)(utc / 1000);
|
||
|
||
lutc = lutc % (3600 * 24);
|
||
|
||
*h = floor((double)(lutc / (3600)));
|
||
*m = floor((lutc - (*h) * 3600) / 60);
|
||
*s = lutc - (*h) * 3600 - (*m) * 60;
|
||
|
||
}
|