navduino

Arduino library for basic aerial navigation functions used for

API:

```#define deg2rad(angle) (angle * M_PI / 180.0)
#define rad2deg(angle) (angle * 180.0 / M_PI)
#define wrapToPi(e)    (fmod(e + M_PI, 2 * M_PI) - M_PI)

const bool DEGREES = false;

const bool NED_TO_BODY = true;
const bool BODY_TO_NED = false;

const int dRx = 0; // w.r.t. rotation around the x axis
const int dRy = 1; // w.r.t. rotation around the y axis
const int dRz = 2; // w.r.t. rotation around the z axis
const int dtx = 3; // w.r.t. translation around the x axis
const int dty = 4; // w.r.t. translation around the y axis
const int dtz = 5; // w.r.t. translation around the z axis

// Rotation conversions
Matrix3f angle2dcm(const Vector3f& angles, const bool& angle_unit = DEGREES, const bool& NED_to_body = true, const int& rotation_sequence = 321);
Vector3f dcm2angle(const Matrix3f& dcm, const bool& angle_unit = DEGREES, const bool& NED_to_body = true, const int& rotation_sequence = 321);
Quaternionf angle2quat(const Vector3f& angles, const bool& angle_unit = DEGREES, const bool& NED_to_body = true, const int& rotation_sequence = 321);
Vector3f quat2angle(const Quaternionf& quat, const bool& angle_unit = DEGREES, const bool& NED_to_body = true, const int& rotation_sequence = 321);
Matrix3f quat2dcm(const Quaternionf& quat);
Quaternionf dcm2quat(const Matrix3f& dcm);
Matrix3f vec2dcm(const Vector3f& vec);
Vector3f dcm2vec(const Matrix3f& dcm);
Quaternionf vec2quat(const Vector3f& vec);
Vector3f quat2vec(const Quaternionf& quat);
Vector3f vec2angle(const Vector3f& vec, const bool& angle_unit = DEGREES, const bool& NED_to_body = true, const int& rotation_sequence = 321);
Vector3f angle2vec(const Vector3f& angles, const bool& angle_unit = DEGREES, const bool& NED_to_body = true, const int& rotation_sequence = 321);

float earthGeoRad(const float& _lat, const bool& angle_unit = DEGREES);
Vector2f earthRad(const float& _lat, const bool& angle_unit = DEGREES);
float earthAzimRad(const float& lat, const float& _azimuth, const bool& angle_unit = DEGREES);

// Frame angular rate calculations
Vector3f earthRate(const float& _lat, const bool& angle_unit = DEGREES);
Vector3f llaRate(const Vector3f& vned, const Vector3f& lla, const bool& angle_unit = DEGREES);
Vector3f navRate(const Vector3f& vned, const Vector3f& lla, const bool& angle_unit = DEGREES);

// Frame conversions
Vector3f lla2ecef(const Vector3f& lla, const bool& angle_unit = DEGREES);
Vector3f ecef2lla(const Vector3f& ecef, const bool& angle_unit = DEGREES);
Matrix3f ecef2ned_dcm(const Vector3f& lla, const bool& angle_unit = DEGREES);
Vector3f ecef2ned(const Vector3f& ecef, const Vector3f& lla_ref, const bool& angle_unit = DEGREES);
Vector3f lla2ned(const Vector3f& lla, const Vector3f& lla_ref, const bool& angle_unit = DEGREES);
Vector3f ned2ecef(const Vector3f& ned, const Vector3f& lla_ref, const bool& angle_unit = DEGREES);
Vector3f ned2lla(const Vector3f& ned, const Vector3f& lla_ref, const bool& angle_unit = DEGREES);

// Pose functions
Matrix4f poseMat(const Matrix3f& dcm, const Vector3f& t);
Matrix3f pose2dcm(const Matrix4f& poseMatrix);
Vector3f pose2t(const Matrix4f& poseMatrix);
Matrix4f reversePoseMat(const Matrix4f& poseMatrix);
Matrix4f poseMatDeriv(const Matrix4f& poseMatrix, const int& derivType);
Vector3f transformPt(const Matrix3f& dcm, const Vector3f& t, const Vector3f& x);
Vector3f transformPt(const Matrix4f& poseMatrix, const Vector3f& x);

// Linear algebra
Matrix3f skew(const Vector3f& w);

// Bearing calculations
float bearingLla(const Vector3f& lla_1, const Vector3f& lla_2, const bool& angle_unit = DEGREES);
float bearingNed(const Vector3f& ned_1, const Vector3f& ned_2);

// Distance calculations
double distanceLla(const Vector3f& lla_1, const Vector3f& lla_2, const bool& angle_unit = DEGREES);
float distanceNed(const Vector3f& ned_1, const Vector3f& ned_2);
float distanceNedHoriz(const Vector3f& ned_1, const Vector3f& ned_2);
float distanceNedVert(const Vector3f& ned_1, const Vector3f& ned_2);
float distanceEcef(const Vector3f& ecef_1, const Vector3f& ecef_2);

// Elevation calculations
float elevationLla(const Vector3f& lla_1, const Vector3f& lla_2, const bool& angle_unit = DEGREES);
float elevationNed(const Vector3f& ned_1, const Vector3f& ned_2);

// Relative coordinate calculations
Vector3f LDAE2lla(const Vector3f& lla, const float& dist, const float& _azimuth, const float& _elevation = 0, const bool& angle_unit = DEGREES);
Vector3f NDAE2ned(const Vector3f& ned, const float& dist, const float& _azimuth, const float& _elevation = 0, const bool& angle_unit = DEGREES);

// Eigen object printing functions
void printVec2f(const Vector2f& vec, const int& p = 5, Stream& stream = Serial);
void printVec3f(const Vector3f& vec, const int& p = 5, Stream& stream = Serial);
void printVec4f(const Vector4f& vec, const int& p = 5, Stream& stream = Serial);
void printQuatf(const Quaternionf& quat, const int& p = 5, Stream& stream = Serial);
void printMat3f(const Matrix3f& mat, const int& p = 5, Stream& stream = Serial);
void printMat4f(const Matrix4f& mat, const int& p = 5, Stream& stream = Serial);

// Constrain functions
float float_constrain(const float& input, const float& min, const float& max);
double double_constrain(const double& input, const double& min, const double& max);

// Map functions
float float_map(const float& x, const float& in_min, const float& in_max, const float& out_min, const float& out_max);
double double_map(const double& x, const double& in_min, const double& in_max, const double& out_min, const double& out_max);

class vehicle_pose
{
public:
vehicle_pose(const byte& id) { _vehicleID = id; };

byte vehicleID() { return _vehicleID; };

Vector3f e_t_e_v() { return _e_t_e_v; };
Vector3f v_t_v_e() { return -_e_t_e_v; };
Matrix3f n_R_e() { return _n_R_e; };
Matrix3f e_R_n() { return _n_R_e.transpose(); };
Matrix3f v_R_n() { return _v_R_n; };
Matrix3f n_R_v() { return _v_R_n.transpose(); };
Matrix4f n_P_e() { return _n_P_e; };
Matrix4f e_P_n() { return reversePoseMat(_n_P_e); };
Matrix4f v_P_e() { return _v_P_e; };
Matrix4f e_P_v() { return reversePoseMat(_v_P_e); };

Vector3f lla() { return _lla; };
Vector3f ecef() { return lla2ecef(_lla, DEGREES); };
Vector3f euler() { return dcm2angle(_v_R_n, DEGREES, NED_TO_BODY, 321); };
Quaternionf quat() { return dcm2quat(_v_R_n); };

Vector3f body2ned(const Vector3f& x) { return _v_R_n.transpose() * x; };
Vector3f ned2body(const Vector3f& x) { return _v_R_n * x; };
Vector3f body2ecef(const Vector3f& x) { return transformPt(e_P_v(), x); };
Vector3f ecef2body(const Vector3f& x) { return transformPt(_v_P_e, x); };
Vector3f body2lla(const Vector3f& x) { return ned2lla(_v_R_n.transpose() * x, _lla, DEGREES); };
Vector3f lla2body(const Vector3f& x) { return _v_R_n * lla2ned(x, _lla, DEGREES); };

void update_dcm(const Matrix3f& _v_R_n_);
void update_loc_lla(const Vector3f& _lla_, const bool& angle_unit = DEGREES);
void update_loc_ecef(const Vector3f& _ecef_);

private:
byte _vehicleID;

Vector3f _lla; // dd, dd, m

Vector3f _e_t_e_v; // translation vector (in m) from ECEF frame to vehicle's local level/NED frame in the ECEF frame
Matrix3f _n_R_e;   // dcm from ECEF frame to vehicle's local level/NED frame
Matrix3f _v_R_n;   // dcm from vehicle's local level/NED frame to vehicle's body frame
Matrix4f _n_P_e;   // pose matrix that maps points from the ECEF frame to the vehicle's local level/NED frame https://en.wikipedia.org/wiki/Affine_transformation
Matrix4f _v_P_e;   // pose matrix that maps points from the ECEF frame to the vehicle's body frame https://en.wikipedia.org/wiki/Affine_transformation
};

{
public:
payload_pose(const byte& vid, const byte& pid) { _vehicleID = vid; _payloadID = pid; };

byte vehicleID() { return _vehicleID; };

Vector3f v_t_v_p() { return _v_t_v_p; };
Vector3f p_t_p_v() { return -_v_t_v_p; };
Matrix3f p_R_v() { return _p_R_v; };
Matrix3f v_R_p() { return _p_R_v.transpose(); };
Matrix4f p_P_v() { return _p_P_v; };
Matrix4f v_P_p() { return reversePoseMat(_p_P_v); };

void update_v_t_v_p(const Vector3f& _v_t_v_p_);
void update_p_t_p_v(const Vector3f& _p_t_p_v_);
void update_p_R_v(const Matrix3f& _p_R_v_);
void update_v_R_p(const Matrix3f& _v_R_p_);
void update_p_P_v(const Matrix4f& _p_P_v_);
void update_v_P_p(const Matrix4f& _v_P_p_);

private:
byte _vehicleID;

Vector3f _v_t_v_p; // translation vector (in m) from vehicle CG to payload in the vehicle's body frame
Matrix3f _p_R_v;   // dcm from vehicle's body frame to payload's body frame
Matrix4f _p_P_v;   // pose matrix that maps points from the vehicle's body frame to the payload's body frame https://en.wikipedia.org/wiki/Affine_transformation
};

class sensor_pose
{
public:
sensor_pose(const byte& vid, const byte& pid, const byte& sid) { _vehicleID = vid; _payloadID = pid; _sensorID = sid; };

byte sensorID() { return _sensorID; };

Vector3f p_t_p_s() { return _p_t_p_s; };
Vector3f s_t_s_p() { return -_p_t_p_s; };
Matrix3f s_R_p() { return _s_R_p; };
Matrix3f p_R_s() { return _s_R_p.transpose(); };
Matrix4f s_P_p() { return _s_P_p; };
Matrix4f p_P_s() { return reversePoseMat(_s_P_p); };

void update_p_t_p_s(const Vector3f& _p_t_p_s_);
void update_s_t_s_p(const Vector3f& _s_t_s_p_);
void update_s_R_p(const Matrix3f& _s_R_p_);
void update_p_R_s(const Matrix3f& _p_R_s_);
void update_s_P_p(const Matrix4f& _s_P_p_);
void update_p_P_s(const Matrix4f& _p_P_d_);

private:
byte _vehicleID;
byte _sensorID;

Vector3f _p_t_p_s; // translation vector (in m) from payload to sensor in the payload's body frame
Matrix3f _s_R_p;   // dcm from payload's body frame to sensor's body frame
Matrix4f _s_P_p;   // pose matrix that maps points from the payload's body frame to the sensor's body frame https://en.wikipedia.org/wiki/Affine_transformation
};

Vector3f sensor2vehicle(payload_pose& pay, sensor_pose& sen, const Vector3f& x);
Vector3f vehicle2sensor(payload_pose& pay, sensor_pose& sen, const Vector3f& x);

Credit:

Navduino is based on the Python library NavPy.

Extra Resources:

PowerPoint on coordinate transforms, pose transforms, and rotations

Releases(0.5.1)
PB2
Electrical Engineer (Virginia Tech alumnus)
A run-time C++ library for working with units of measurement and conversions between them and with string representations of units and measurements

Units What's new Some of the CMake target names have changed in the latest release, please update builds appropriately Documentation A library that pr

106 Jul 22, 2022
Libft is an individual project at 42 that requires us to re-create some standard C library functions including some additional ones that can be used later to build a library of useful functions for the rest of the program.

Libft is an individual project at 42 that requires us to re-create some standard C library functions including some additional ones that can be used later to build a library of useful functions for the rest of the program.

0 Apr 5, 2022
Basic definitions and utility functions for GNSS raw measurement processing

gnss_comm Authors/Maintainers: CAO Shaozu (shaozu.cao AT gmail.com) The gnss_comm package contains basic definitions and utility functions for GNSS ra

55 Jul 20, 2022
this is very basic version for our dataset validation, only change the path, and align the frame of vio and Groundtruth

VINS-Fusion for UrbanNavDataset Evaluation 1. Prerequisites please refer to VINS-Fusion Github 2. Build mkdir catkin/src cd catkin/src mkdir result cd

12 May 26, 2022
Itpp - IT++ library mirror/fork. C++ library of mathematical, signal processing and communication classes and functions.

Introduction ************ IT++ is a C++ library of mathematical, signal processing and communication classes and functions. Its main use is in simula

18 Apr 9, 2022
Connected Santa Claus hat based on an ESP32, 8x8 matrices and LEDs stripped communicating over the internet with MQTT.

Protobonnet What is it? A connected Santa Claus hat! What is it called? Le ProtoBonnet! Why? This hat has been created entirely for the Noël des proto

2 Dec 27, 2021
Collage assingment No.2 in C; matrices and linked lists

C-Advanced-Assingment-2 *Note that you have to edit the returned student ID on each file. You might consider make some changes before submitting these

3 May 31, 2022
An 802.11 Frame Generation and Parsing Library in C

libwifi 802.11 Parsing / Generation library Build Status OS Architecture Linux x86_64 What is this? libwifi is a C library with a permissive license f

28 Jul 27, 2022
Invariant-ekf - C++ library to implement invariant extended Kalman filtering for aided inertial navigation.

inekf This repository contains a C++ library that implements an invariant extended Kalman filter (InEKF) for 3D aided inertial navigation. This filter

239 Jul 25, 2022
Small Robot, with LIDAR and DepthCamera. Using ROS for Maping and Navigation

?? RoboCop ?? Small Robot, with LIDAR and DepthCamera. Using ROS for Maping and Navigation Made by Clemente Donoso, ?? Chile ???? RoboCop Lateral Fron

2 Jan 4, 2022
Icopack - A simple command line tool to create multi-frame ICO files from PNG source images

Optidash is a modern, AI-powered image optimization and processing API. We will drastically speed-up your websites and save you money on bandwidth and

61 Jul 27, 2022
The frame work for WhaleMarket C-Beginner Project.

The frame work for WhaleMarket C-Beginner Project.

28 Jul 25, 2022
8 Jul 30, 2022
3D scanning is becoming more and more ubiquitous.

Welcome to the MeshLib! 3D scanning is becoming more and more ubiquitous. Robotic automation, self-driving cars and multitude of other industrial, med

41 Jul 23, 2022
2D/3D Registration and system integration for image-based navigation of orthopedic robotic applications, inculding femoroplasty, osteonecrosis, etc.

Registration and System Integration Software for Orthopedic Surgical Robotic System This repository contains software programs for image-based registr

14 Jun 11, 2022
An Open Source tripmaster for navigation rallies

Open Rally Computer An open source tripmaster for navigation rallies Description The Open Rally Computer (previously known as Baja Pro) is a complete

44 Jul 9, 2022
Local Navigation Planner for Legged Robots

ANYmal Rough Terrain Planner Sampling based path planning for ANYmal, based on 2.5D height maps. More detailed instructions still to come. The paper d

30 Jul 13, 2022