Integrate asynchronous GPS IMU fusion
This commit is contained in:
@@ -0,0 +1,128 @@
|
||||
#ifndef NAVEQUATIONS_H
|
||||
#define NAVEQUATIONS_H
|
||||
|
||||
#include "gps.h"
|
||||
#include "gps/gps_ip_defines.h"
|
||||
#include "gps/matrixLib.h"
|
||||
|
||||
typedef struct gps_nav_solution {
|
||||
double ecef_m[3];
|
||||
double clock_bias_m;
|
||||
double latitude_deg;
|
||||
double longitude_deg;
|
||||
double height_m;
|
||||
double covariance_enu_m2[3][3];
|
||||
double hacc_m;
|
||||
double vacc_m;
|
||||
double gdop;
|
||||
double pdop;
|
||||
double hdop;
|
||||
double vdop;
|
||||
double tdop;
|
||||
double residual_rms_m;
|
||||
double max_abs_residual_m;
|
||||
double postfit_residual_m[GPS_TRCK_IP_COUNT];
|
||||
double elevation_deg[GPS_TRCK_IP_COUNT];
|
||||
double measurement_sigma_m[GPS_TRCK_IP_COUNT];
|
||||
double full_postfit_residual_m[GPS_TRCK_IP_COUNT];
|
||||
double full_elevation_deg[GPS_TRCK_IP_COUNT];
|
||||
double full_measurement_sigma_m[GPS_TRCK_IP_COUNT];
|
||||
double weighted_residual_rms_m;
|
||||
double weighted_residual_sum;
|
||||
double chi_square_threshold;
|
||||
double chi_square_ratio;
|
||||
double full_chi_square_ratio;
|
||||
double postfit_position_correction_m;
|
||||
double postfit_clock_correction_m;
|
||||
double full_postfit_position_correction_m;
|
||||
double full_postfit_clock_correction_m;
|
||||
double quality_score;
|
||||
unsigned int available_satellite_count;
|
||||
unsigned int used_satellite_count;
|
||||
unsigned int tracked_satellite_count;
|
||||
unsigned int degrees_of_freedom;
|
||||
uint32_t used_row_mask;
|
||||
uint32_t excluded_row_mask;
|
||||
uint64_t candidate_cycles;
|
||||
uint64_t full_solution_cycles;
|
||||
uint64_t fde_cycles;
|
||||
uint64_t total_wls_cycles;
|
||||
unsigned int iteration_count;
|
||||
unsigned int fde_trial_count;
|
||||
bool candidate_valid;
|
||||
bool residual_test_valid;
|
||||
bool chi_square_pass;
|
||||
bool fde_applied;
|
||||
bool solution_valid;
|
||||
} gps_nav_solution_t;
|
||||
|
||||
void satpos(gps_ephemeris_t* eph, double* pos, double* vel, double* clk, double* clockCorr);
|
||||
void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_ephemeris_t* eph, ionoutc_t* ionoutc, double* xyz, t_temp_pos* tempPos, double* clockCorr, mat_type *satellitePositions, mat_type * velocities);
|
||||
double check_t(long double tkl);
|
||||
void subVect(double* y, const double* x1, const double* x2);
|
||||
double normVect(const double* x);
|
||||
void minimum_element(double* minimum, const double * vec, const int ActiveSatellitesNumber);
|
||||
bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber,
|
||||
mat_type * az, mat_type * el,
|
||||
const unsigned char* ura_indices,
|
||||
const ionoutc_t* ionoutc, double gps_tow,
|
||||
gps_nav_solution_t* solution_out);
|
||||
void rotateSatelliteSTD(double const traveltime, const mat_type* X_sat, mat_type* X_sat_rot);
|
||||
void topocent(const double* XPtr, const double* dxPtr, int size, t_phi_Lambda_h * phi_Lambda_h, t_topocentData* topcent);
|
||||
void togeod(double a, double finv, double X, double Y, double Z, t_phi_Lambda_h * phi_Lambda_h, t_topocentData* topcent);
|
||||
double tropo(double sinel, double hsta);
|
||||
double SquareRoot(double number);
|
||||
void cart2geo(double X, double Y, double Z, int i, t_phi_Lambda_h * phi_Lambda_h);
|
||||
int findUtmZone(double latitude, double longitude);
|
||||
|
||||
//typedef struct{
|
||||
// mat_type* absoluteSamples;
|
||||
// mat_type* txTimeVec;
|
||||
// mat_type* pseudoRanges;
|
||||
// mat_type* az;
|
||||
// mat_type* el;
|
||||
//}matc_variable_t; // matc class variable encapsulation
|
||||
// ***HEADER VARIABLES*** ( class variables that MAPPED TO THE HEADER GLOBAL VARIABLES )
|
||||
//extern matc_variable_t* m_matc_variable;
|
||||
//t_topocentData topcent;
|
||||
//extern unsigned int nmbOfIterations;
|
||||
//extern char subframeID; //TODO where is this variable get initialized ?
|
||||
//extern unsigned int number_of_satellites; //TODO it will be replaced with subframeID; (represents same thing. # of captured satellites)
|
||||
//extern mat_type* pos; // position vector.
|
||||
//extern mat_type* posEq; // ?
|
||||
//extern t_phi_Lambda_h phi_lambda_h;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//double* mat(int n, int m);
|
||||
////double absoluteSamples[6] = {66564551, 66525117,66589459,66666672,66633917,66700000};
|
||||
//double travelTime[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
|
||||
//double pseudoRanges[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
|
||||
//double az[8] = {0,0,0,0,0,0,0,0};
|
||||
//double el[8] = {0,0,0,0,0,0,0,0};
|
||||
//double pos[12] = {0,0,0,0,0,0,0,0,0,0,0,0};
|
||||
//double CorrectedPos[6] = {0,0,0,0,0,0};
|
||||
//double Positions[MAX_SAT][POSITION_VECTOR_COL];
|
||||
|
||||
//double subGpsTime(gpstime_t g1, gpstime_t g0);
|
||||
//gpstime_t incGpsTime(gpstime_t g0, double dt);
|
||||
//void ecef2neu(const double* xyz, double t[3][3], double* neu);
|
||||
//void ltcmat(const double* llh, double t[3][3]);
|
||||
//void neu2azel(double* azel, const double* neu);
|
||||
//void gps2date(const gpstime_t* g, datetime_t* t);
|
||||
//void llh2xyz(const double* llh, double* xyz);
|
||||
|
||||
|
||||
//double dotProd(const double* x1, const double* x2);
|
||||
|
||||
//void xyz2llh(const double* xyz, double* llh);
|
||||
//double ionosphericDelay(const ionoutc_t* ionoutc, gpstime_t g, double* llh, double* azel);
|
||||
|
||||
//int findUtmZone(double latitude, double longitude);
|
||||
//void CART2UTM(double X, double Y, double Z, int zone);
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user