logzhan
/
PDR
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
PDR/1.Software/PDR 1.06/src/Utils.c

466 lines
13 KiB
C
Raw Normal View History

整理计步器
2023-02-25 22:43:50 +08:00
/******************** (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 <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ຯ<EFBFBD><EFBFBD>֧<EFBFBD><EFBFBD>
********************************************************************************/
/* 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; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>ֵ
/* Function Declaration ------------------------------------------------------*/
/**----------------------------------------------------------------------
* Function : mean
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>double<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
* Review : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD>ζ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD><EFBFBD>ռ䣬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <EFBFBD><EFBFBD>ͬ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* 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 : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>float<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
* Review : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD>ζ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ż<EFBFBD><EFBFBD>ռ䣬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* <EFBFBD><EFBFBD>ͬ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* 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 : <EFBFBD><EFBFBD><EFBFBD><EFBFBD>1/sqrt(x)<EFBFBD>Ŀ<EFBFBD><EFBFBD>ٵ<EFBFBD><EFBFBD>㷨
* 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 : <EFBFBD><EFBFBD>ά<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD>
* Date : 2022/11/1 logzhan
*---------------------------------------------------------------------**/
void Vec3Norm(float* vx, float* vy, float* vz)
{
float norm = sqrtf(((*vx) * (*vx) + (*vy) * (*vy) +
(*vz) * (*vz)));
// <20><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>ģΪ0<CEAA><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if (norm > FLT_THRES) {
norm = 1 / norm;
(*vx) *= norm; (*vy) *= norm; (*vz) *= norm;
}
}
/**----------------------------------------------------------------------
* Function : Motion2TypeStr
* Description : <EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD>˶<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>Ϊ<EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* 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 : <EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD>תŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* 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: <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɾ<EFBFBD>γ<EFBFBD>ȱ<EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD>߾<EFBFBD><EFBFBD>롣
param @ pointA : <EFBFBD><EFBFBD>A<EFBFBD>ľ<EFBFBD>γ<EFBFBD><EFBFBD>
param @ pointB : <EFBFBD><EFBFBD>B<EFBFBD>ľ<EFBFBD>γ<EFBFBD><EFBFBD>
return @ dis : A<EFBFBD><EFBFBD>B<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD><EFBFBD>룻<EFBFBD><EFBFBD>С<EFBFBD><EFBFBD>0<EFBFBD><EFBFBD>˵<EFBFBD><EFBFBD>A<EFBFBD><EFBFBD>B<EFBFBD><EFBFBD><EFBFBD>ľ<EFBFBD>γ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
*/
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 <20><> point <20><> x <20>ݶ<EFBFBD> */
double gradxB_A; /* linePointA <20><> linePointB <20><> x <20>ݶ<EFBFBD> */
double gradyB_A; /* linePointA <20><> linePointB <20><> y <20>ݶ<EFBFBD> */
double normPow; /* linePointA <20><> linePointB <20>ݶȵ<DDB6>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD> */
double scalarMultip; /* gradxB_A * gradyB_A <20>ı<EFBFBD><C4B1><EFBFBD><EFBFBD>˷<EFBFBD> */
LatLng_t resProjPoint; /* <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
/* 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))
{
/* <20><>linePointA <20><>linePointB<74><42>ͬһ<CDAC><D2BB>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD>point<6E><74><EFBFBD><EFBFBD> */
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))
{
/* <20><>linePointA <20><> linePointB <20><><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD> x<><78><EFBFBD><EFBFBD>y<EFBFBD><79>ƽ<EFBFBD><C6BD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
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 : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>γ<EFBFBD>ȷ<EFBFBD><EFBFBD>ظ<EFBFBD>γ<EFBFBD>ȵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ز<EFBFBD><EFBFBD><EFBFBD>
* 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 : <EFBFBD><EFBFBD><EFBFBD>㻡<EFBFBD><EFBFBD>֮<EFBFBD><EFBFBD>
* 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 : <EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣд<EFBFBD><EFBFBD><EFBFBD>ı<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD>Ƕ<EFBFBD>fprintf<EFBFBD><EFBFBD>װһ<EFBFBD><EFBFBD>
* 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 : <EFBFBD><EFBFBD>UTCʱ<EFBFBD><EFBFBD>ת<EFBFBD><EFBFBD>Ϊʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* 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;
}