NAVFUSION Documentation

Documentation of Navfusion: GPS + INS sensor fusion.
Nicolas Padron. Copyright 2024.
https://www.spacewayfinder.com

Usage Description

The program reads the command line (can be set by help of /tools/run.py, see Tools section and Usage in readme.md), as well as an input CSV file. The output is an analysis CSV file, as well as KML files for GPS, INS and FUSION results.

Note that the program expects a CSV file with decimals represented with dots: “0.1,0.5,…”.

Folder Architecture

/src: C++ source files.
/tools: Python helper functions for running Tools.
/libs: default Armadillo library. This is by default set in /src/CMakeLists.txt. See Installing Armadillo section on readme.md for more information.
/out: directory with execution file already built. This is by default executed by /tools/run.py.
/build: directory where user defined builds can be done.
/data: default folder to store CSV files to be used as input, and write outputs: CSV and KML.

Interface

This takes charge of input/output file reading/writing (/io), command line reading (/ui) and parsing into corresponding navigation control flow data (/navdata).

io

interface/io/files/io_files

class FileHandler

Class to handle files for read/write.

Public Functions

inline FileHandler(): Default constructor

bool openFile(void): Open individual file with status checkings

bool closeFile(void): Close individual fole with status checkings

int readLine(std::string &line): Read file line

int writeContent(const char *str): Write into file

void setOpenOption(int opt): Set open mode

void setFilename(const std::string &filename_): Set filename

const std::string &getFilename(void) const: Get filename

long getFileSize(void): Get file size in bytes

long getReadBytes(void): Get number of bytes already read

const IoFilesAction_e getFileLastAction(void): Get file last action

class InputCsvFields

Class to handle the Input CSV fields.

Public Functions

inline InputCsvFields(): Default constructor

inline InputCsvFields(std::string fieldname_, double fieldvalue_): Constructor used when inserting the CSV into the input map in Input::readLine

Public Members

std::string fieldname: Variable to hold field name in input CSV

double fieldvalue: Variable to hold field value in input CSV

interface/io/in/io_in

class Input

Read/Write Files Interface class.

Public Functions

void openIOFiles(void): Open files entered as Input

void closeFiles(void): Close files

bool readline(bool jumpLine = false, int epochCounter = 0): Read input line

void readInputCsvIds(void): Function to read the IDs of the input CSV file

const double getFieldvalue(int id): Get the fieldvalues

Public Static Functions

static Input &getInstance(void): Singleton class, function returns static object from private constructor

static const string removeStartingWhiteSpace(const string filename): Remove white space on string, needed for UI

interface/io/out/io_out

class Output_c

to add the output to the std::map to write, and read it back.

Public Functions

void writeHeaders(void): Write headers: for both analysis CSV and KMLs

void writeContent(void): Write content: for both analysis CSV and KMLs

void kmlWriteFooter(void): Write footer for KMLs. Cannot be handled together with analysis CSV like header and content.

Public Static Functions

static Output_c &getInstance(void): Singleton class, function returns static object from private constructor

ui

interface/ui/ui

class InputIds: Class stores the indexes corresponding to the CSV columns where data is stored.

class InputCmdLine

Class to handle the input command line.

Public Functions

InputCmdLine &operator=(const InputCmdLine&) = delete: Singleton class, delete copy constructor

void checkInputCmdLine(const std::vector<std::string> inputCmdLineStr, bool *cmdArgRequiredEntered)

Check if the required argument is within the entered arguments.

Parameters:

inputCmdLineStr – Input comand line to be parsed to the map.
cmdArgRequiredEntered – Bool indicating whether the input arguments are valid or not.

void readInputCmdLine(const std::vector<std::string> inputCmdLineStr, MapInArgs_t &mapInputArgs)

Function for reading the input command line parameters.

Parameters:

inputCmdLineStr – Input comand line to be parsed to the map.
mapInputArgs – Output map with input arguments.

Public Static Functions

static InputCmdLine &getInstance(void): Singleton class, function returns static object from private constructor

class UI

Class to handle the Input Interface with the user. In charge of managing handling the command line and parsing into an internal map. Mainly store the filenames, config input variables used in navigation data, described in InputValues_t, and columns descrived in InputIds to be used in NavDataInterface.

Public Functions

UI &operator=(const UI&) = delete: Singleton class, delete copy constructor

void start(const int argc_, char *argv_[])

UI starting point. In charge of loading the default parameters and reading the input command line to the input argument map.

Parameters:

argc_ – Number of input arguments.
argv_ – Array of input arguments.

MapInArgs_t getMapInputArgs(void): Call method for function to get the mapInputArgs variable

void loadParams(): Loops through the map containing the input command line arguments, and fill the internal data before starting processing

InputIds &getInputIds(void): Get the IDs (= CSV column indexes) of the data in the CSV entered in command line

const InputValues_t &getInputValues(void) const: Get the input values entered in the command line, these are values which are not read from the CSV but directly entered by the user These values are then copied to NavDatInterface clasas to centralize all navigation-related data from there.

Public Static Functions

static UI &getInstance(void): Singleton class, function returns static object from private constructor

static void ui_usage(void): Explain how to use the software

navdata

interface/navdata/interface_navdata

class MapInputMonitorStruct

Input monitor struct for each Key. The idea is to parse every input field with an id (user entered CSV column number) and store the corresponding value.

Public Functions

inline MapInputMonitorStruct(): Constructor

inline MapInputMonitorStruct(arma::Mat<int> in_inputId, arma::vec in_inputHolder = arma::vec({0, 0, 0}))

Constructor used to insert/create new entry in NavDataInterface::Initialize function.

Parameters:

in_inputId – ID number, i.e., column number in CSV/Excel
in_inputHolder – actual value. It is entered as a 3D array: XYZ (for accelerometers, gyrometers and magnetometers), radians (for LAT/LON, or attitude angles).

class NavDataInterface

Input data file monitor class. Its purpose is to manage the input data to be used for navigation. It is not the same as UI input or general Input class handler.

Public Functions

void initialize(void): Initialize interface. Creates MapInputMonitorStruct entries.

void update(void): Update at every new input file row, i.e., every unit of time. Updates the map field in MapInputMonitorStruct entries.

const MapInputMonitor_t &getMapInputMonitor(void) const: Function to retrieve private MapInputMonitorStruct

const InputValues_t &getInputValues(void) const: Function to retrieve user entered input values with navigation purpose (different to the general UI entered, which are for example filenames or CSV columns)

const bool getIsGpsDataNew(void) const: Function to retrieve the bool checking if GPS data is new, in which case the KF is updated. A getter function for a single variable might be excessive, but this way we can ensure that the value is not modified anywhere else

const bool getIsGpsDataValid(void) const: Function to retreive the bool checking if GPS data is valied, i.e. non-nan.

const int getEpochCounter(void) const: Get epoch counter

Public Static Functions

static NavDataInterface &getInstance(): Singleton class, function returns static object from private constructor

processing

This takes charge of each module processing (GPS, INS and Fusion), as well as handles the Frames (namespace for frame handling, matrix rotation, etc.) and Kalman Filter.

Frames

processing/frames

namespace Frames

Frames namespace. This contains the functions to convert between coordinate frames. It is intended to be a class of the object whose coordinates we want to handle, since it stores parameters related to that object (GPS, INS, FUSION)

Functions

arma::vec llh2ecef(const arma::vec llh)

LLH to ECEF conversion (WGS84). Reference: Understanding GPS Principles and Applications 2.2.3 World Geodetic System.

Parameters:: llh – input LLH coordinates to convert to ECEF.
Returns:: ECEF coordinates

arma::vec ecef2llh(const arma::vec ecef)

ECEF to LLH conversion (WGS84). Reference: Understanding GPS Principles and Applications 2.2.3 World Geodetic System.

Parameters:: ecef – input ECEF coordinates to convert to LLH.
Returns:: LLH coordinates

arma::vec ecef2enu(const arma::vec llh, const arma::vec ecef, const arma::vec xyz0)

ECEF to ENU conversion.

Parameters:

llh – input needed to pefrorm ECEF rotation
ecef – input ECEF coordinates to convert to ENU.
xyz0 – input corresponding to 1st ECEF location, i.e. reference to compute ENU.

Returns:

ENU coordinates

arma::vec enu2ecef(const arma::vec llh, const arma::vec enu, const arma::vec xyz0)

ENU to ECEF conversion.

Parameters:

llh – input needed to pefrorm ECEF rotation
enu – input ENU coordinates to convert to ECEF.
xyz0 – input corresponding to 1st ECEF location, i.e. reference to compensate.

Returns:

ECEF coordinates

arma::Mat<double> matrixEcef2Enu(const arma::vec llh)

Generate matrix to rotate from ECEF to ENU.

Parameters:: llh – input llh coordinates.
Returns:: 3x3 rotation matrix

arma::Mat<double> matrixBody2Enu(const arma::vec rpy)

Generate matrix to rotate from Body (XYZ) to ENU plane.

Parameters:: rpy – input Roll, Pitch and Yaw 3x1 angles array.
Returns:: 3x3 rotation matrix

arma::mat matrixBody2H(const arma::vec rpy)

Generate matrix to align to horizontal plane.

Parameters:: rpy – input Roll, Pitch and Yaw 3x1 angles array.
Returns:: 3x3 rotation matrix

arma::mat matrixPlatform2Body(const arma::vec rows)

Generate matrix to align platform to body.

Parameters:: rows – concatenated rows as 9x1 vector. First 3x1 are 1st row, 2nd 4x1 are 2nd row and so on.
Returns:: 3x3 rotation matrix

arma::Mat<double> genRotRx(const float angle)

Generate rotation matrix around X axis, which is later rotated/shifted to generate Y and Z axes rotation.

Parameters:: angle – input angle to use for rotation
Returns:: 3x3 rotation matrix

arma::mat matrixRateAttitudeDynamics(const arma::vec rpy)

Generate matrix with attitude angles rates.

Parameters:: rpy – current attitude angles
Returns:: 3x3 rotation matrix

arma::mat skew(const arma::vec x)

Form skwe matrix.

Parameters:: x – 3x1 vector of inputs.
Returns:: 3x3 skew matrix

void adjustYaw(double &yaw)

Adjust yaw to be within [0,360] (in radians).

Parameters:: yaw – yaw angle.

void adjustRollPitch(double &x)

Adjust roll or pitch to be within [-90,+90] (in radians).

Parameters:: x – angle

const arma::mat skewInertialEarth(const double lat)

Get skew matrix of Earth frame relative to inertial frame.

Parameters:: lat – latitude angle.
Returns:: skew symmetric matrix.

double gravityCorrectionForComponentZ(double lat, double hei)

Correct for gravity in Z direction following [Farrel and Barth 1999].

Correct gravity for zenith component

Parameters:

lat – latitude.
hei – height.

Returns:

gravity to compensate for.

Variables

const double SEMI_MAJOR_A = 6378137

const double SEMI_MINOR_B = 6356752.3142

const double ECC = 0.08181919084261345

const double ECC_SEC = 0.0820944379497174

const double SPEED_LIGHT = 299792458

const double G_EQUATOR = 9.78032677

const double G_POLES = 9.83218636

const double PI = 3.141592653589793

const int PI_16 = ((int)(PI * 65536))

const double RAD2DEG = 180 / PI 

const double DEG2RAD = PI / 180

const double EARTH_ROTATION = 0.00007292115

const double ADJUST_ANGLE_MARGIN = 0.001

Systems

Datatype Template

template<class Datatype> class SystemDataTemplate

General template class to handle system datatypes: holds datatype and access functions to it. Used in datatypes for GNSS, IMU (= GPS + inertial) and Fusion (= IMU). Also KF datatype (= KF vectors and matrices).

Public Functions

const Datatype &getData(void) const: Get data by reference

processing/system/gnss

class GnssMain : public SystemDataTemplate<DatatypesGps_t>

Class to handle GNSS processing.

Public Functions

void process(void): System processing. Mainly responsible for frame conversion and setting ECEF reference, since main processing is in INS and FUSION classes.

processing/system/ins

class InsMain : public SystemDataTemplate<DatatypesIns_t>

Class to handle INS system.

Public Functions

void process(void): System processing. Responsible to handle attitude angles to compute intertial navigation, and convert from ENU to LLH coordinates

processing/system/fusion

class FusionMain : public SystemDataTemplate<DatatypesFusion_t>

Class to handle Fusion system.

Public Functions

void initialize(void): Fusion system initialization

void process(void): System processing. Responsible to call KF to compute fused state with GPS and IMU data and convert from ENU to LLH coordinates

const DatatypesKF_t &getKfState(void): Return const reference to KF variables to be accessed read-only from other modules.

processing/system/kf

class KalmanFilter : public SystemDataTemplate<DatatypesKF_t>

Class to handle Kalman Filter. Not technically needed to be singletoon class, although only one object is created. Inherits SystemDataTemplate methods to access KF variables from outside. KF variables are of type DatatypesKF_t.

Public Functions

void initialize(void): KF initialization

template<class DatatypePrediction_s, class DatatypeObservation_s> void process(const DatatypePrediction_s &sDataIns, const DatatypeObservation_s &sDataGps, const bool isKfUpdatable)

Process function, responsible to call internal prcesses to form state transition matrix, discretize, predict state and update filter. It works with two datatypes: IMU and GNSS, although they were kept in general form here (reason to be a template function).

Parameters:

sDataIns – input prediction data, in this case is a high rate sensor, IMU.
sDataGps – input observation data, in this case a lower rate, GPS. Therefore KF state is updated when GPS is available, and predicted with IMU-only data.
isKfUpdatable – bool to determina if KF is updatable or not, i.e., run prediction only or also update filter.

Tools

Not part of the C++ code, but within /tools, there the following files can be found:

analysis.py : python script to analyze the results. Open script and run editing the input and output filenames.
inputstats.py : quick look in a dataframe about the input data statistics. Mainly computes mean and standard deviation, useful to tune Kalman Filter process and measurement noises.
run.py : helper to run the program. Just edit the fields indicating the values of your data, as well as the corresponding CSV column indexes. You can comment out unused parameters.