![]() |
WarpTwin
Documentation for WarpTwin models and classes.
|
Classes | |
| class | DeadReckon |
| GNC App for performing the propagation step of an EKF when an IMU is available to report the angular velocity and acceleration (without gravity). This app is an implementation if a strapdown IMU based dead-reckoning integration of an objects position, velocity, and attitude relative to some inertial frame (this frame can be pseudoinertial, use your best judgement). This form of propagation step is best used if there are significant external/internal forces other than gravity acting on the system, also forces that cannot be modeled easily as functions of the state vector. More... | |
| class | GPSUpdate |
| GNC App for performing the GPS measurement update step of an EKF when a GPS receiver is available to report the vehicle position in a known reference frame (e.g., ECEF, ECI, or NED). This app implements the EKF measurement correction step by comparing the measured GPS position (after necessary frame and lever-arm transformations) to the predicted position from the EKF state. More... | |
| class | MagUpdate |
| GNC App for performing the magnetometer measurement update step of an EKF when a 3-axis magnetometer is available to report the local magnetic field vector. This app implements the EKF measurement correction step by comparing the measured magnetic field in the magnetometer frame to the expected magnetic field derived from an internal model (e.g., WMM, IGRF, lookup table) and the current state estimate. More... | |
| class | PdAttitudeControl |
| Simple PD attitude controller. More... | |
| class | TwoAxisPointingGuidance |
| Two-axis constrained pointing guidance method guidance. More... | |
| class | LedBlinker |
| LED Blinker example app. More... | |
| struct | PlanetDefaults |
| Contain all default values defined for a planet. More... | |
| class | App |
| Base app class for derived implementation. More... | |
| struct | RegisteredCommandInfo |
| Struct to store info on registered commands. More... | |
| class | CommandManager |
| This class routes commands from the HAL to associated Apps. More... | |
| class | FlightExecutive |
| Executive derivation specifically to run flight systems. More... | |
| class | OS |
| Holds all os-specific interfaces. More... | |
| struct | GpioConfig_t |
| Struct type definition for GPIO configuration interface. More... | |
| struct | SpiConfig_t |
| Struct type definition for SPI configuration interface. More... | |
| struct | UartConfig_t |
| Struct type definition for UART configuration interface. More... | |
| struct | I2cConfig_t |
| Struct type definition for I2C configuration interface. More... | |
| struct | PwmConfig_t |
| Struct type definition for PWM configuration interface. More... | |
| struct | AdcConfig_t |
| Struct type definition for ADC configuration interface. More... | |
| class | Platform |
| Holds all platform-specific interfaces. More... | |
| class | Scheduler |
| Base class implementation of the scheduler. More... | |
| class | Setup |
| Holds the setup configuration for Flight Executive. More... | |
| class | StorageManager |
| App which manages all storage output from the flight system. More... | |
| struct | PacketTypeInfo |
| Telemetry packet type metadata struct. More... | |
| struct | PacketInfo |
| Telemetry packet metadata struct. More... | |
| class | TelemetryManager |
| App which manages all telemetry output from the flight system. More... | |
| class | Rates |
| class | EkfDynamics |
| class | EkfMeasurementUpdate |
| Generic, templated EKF measurement update class. More... | |
| class | EkfTimeUpdate |
| Class to perform EKF time update step. More... | |
| class | GPSMeasurements |
| class | InertialNavigationDynamics |
| class | Measurements |
| class | NonlinearLeastSquares |
| Templated class to perform Nonlinear Least Squares. More... | |
| class | PointingVectorMeasurements |
| class | ForwardEulerIntegrator |
| class | Integrator |
| class | RK4Integrator |
| class | TableScheduler |
| Simple, table-based scheduler for flight software. More... | |
Enumerations | |
| enum | file_descriptors_e : uint8 { WRITE = 0x01 , READ = 0x02 , OPEN_EXISTING = 0x04 , CREATE_NEW = 0x08 , CREATE_ALWAYS = 0x10 , OPEN_ALWAYS = 0x20 , OPEN_APPEND = 0x40 } |
| Options when opening files. More... | |
| enum | tlm_priority_e : uint8 { NORMAL = 0 , HIGH = 1 } |
| Enumeration of valid telemtry queue priorities. More... | |
Functions | |
| int16 | computeCRC16 (uint32 &crc, const uint8 *packet_ptr, size_t length, uint32 input_crc=0) |
| Function to compute the CRC16 checksum for a packet. | |
| int16 | validateCRC16 (const uint32 &crc, const uint8 *packet_ptr, size_t length, uint32 input_crc=0) |
| Function to validate the CRC16 checksum for a packet. | |
| int16 | computeFletcher8Checksum (const uint8 *packet_ptr, uint8 length, uint16 &checksum) |
| Function to calculate the fletcher 8 checksum. | |
| int16 | validateFletcher8Checksum (const uint8 *packet_ptr, uint8 length, const uint16 &checksum) |
| Function to validate the fletcher8 checksum for a packet. | |
| int16 | writeCCSDSHeader (uint8 *header, bool is_tlm, uint16 apid, uint8 seq_flag, uint16 seq_count, uint16 data_len, clockwerk::Time timestamp, uint8 instance) |
| Function to write the CCSDS packet header. | |
| int16 | readCCSDSHeader (uint8 *header, bool &is_tlm, uint16 &apid, uint8 &seq_flag, uint16 &seq_count, uint16 &data_len, clockwerk::Time ×tamp, uint8 &instance) |
| Function to read the CCSDS packet header and extract its contents. | |
| template<class T> | |
| T | changeEndian (T in) |
| Change the endianness of a number. | |
| template<class T, std::size_t N> | |
| std::array< T, N > | changeEndian (const std::array< T, N > &in) |
| Specialized version of changeEndian which manages array values correctly. | |
| EXCLUDE_FROM_COVERAGE (const FlightExecutive &FlightExecutive::operator=(const FlightExecutive &original) { return *this;}) FlightExecutive | |
| void | convertImuInertialCg (const clockwerk::CartesianVector< 3 > &accel_meas_imu_I__imu, const clockwerk::CartesianVector< 3 > &ang_vel_meas_imu_I__imu, const clockwerk::CartesianVector< 3 > &ang_accel_meas_imu_I__imu, const clockwerk::CartesianVector< 3 > &lever_arm_imu_body__body, const clockwerk::Quaternion &quat_imu_body, const clockwerk::CartesianVector< 3 > &accel_bias__imu, const clockwerk::CartesianVector< 3 > &ang_vel_bias__imu, clockwerk::CartesianVector< 3 > &accel_body_I__body, clockwerk::CartesianVector< 3 > &ang_vel_body_I__body) |
| Rotate IMU measurement from IMU frame to inertial body representation. | |
| const uint32 | GPS_MEASUREMENT_VECTOR_ELEMENTS (3) |
| const uint32 | GPS_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS (4) |
| const uint32 | GPS_MEASUREMENT_MEASUREMENT_NOISE_ELEMENTS (3) |
| int | kalman_takasu (floating_point *x, floating_point *P, const floating_point *dz, const floating_point *R, const floating_point *Ht, int n, int m, floating_point chi2_threshold, floating_point *chi2) |
| Kalman Filter Routine (Takasu Formulation). | |
| void | kalman_predict (floating_point *x, floating_point *P, const floating_point *Phi, const floating_point *G, const floating_point *Q, int n, int r) |
| Kalman Temporal / Prediction Step. | |
| int | kalman_udu_scalar (floating_point *x, floating_point *U, floating_point *d, const floating_point dz, const floating_point R, const floating_point *H_line, int n) |
| Square Root Kalman Filter (Bierman) Routine for a single scalar measurement. | |
| int | kalman_udu (floating_point *x, floating_point *U, floating_point *d, const floating_point *z, const floating_point *R, const floating_point *Ht, int n, int m, floating_point chi2_threshold, int downweight_outlier) |
| (Robust) Square Root Kalman Filter (Bierman) update routine for linear systems. | |
| int | decorrelate (floating_point *z, floating_point *Ht, floating_point *R, int n, int m) |
| Decorrelate measurements. For a given covariance matrix R of correlated measurements, calculate a vector of decorrelated measurements (and the matching H-matrix) so that the new covariance R of the decorrelated measurements is an identity matrix. | |
| void | kalman_udu_predict (floating_point *x, floating_point *U, floating_point *d, const floating_point *Phi, const floating_point *G, const floating_point *Q, int n, int r) |
| UDU' (Thornton) Filter Temporal / Prediction Step. | |
| void | matmul (const char *ta, const char *tb, int n, int k, int m, floating_point alpha, const floating_point *A, const floating_point *B, floating_point beta, floating_point *C) |
| void | matmulsym (const floating_point *A_sym, const floating_point *B, int n, int m, floating_point *C) |
| Multiplication of symmetric matrix A with B: C = A*B. | |
| void | mateye (floating_point *A, int n) |
| Fill array with an identity matrix. | |
| int | cholesky (floating_point *A, const int n, int onlyWriteLowerPart) |
| Calculate the lower triangular matrix L, so that L*L' = A. Operation count: n^3/6 with n square roots. BLAS equivalent: ?potrf. | |
| void | trisolve (const floating_point *A, floating_point *B, int n, int m, const char *tp) |
| Triangular solve BLAS: ?trsm. | |
| void | trisolveright (const floating_point *L, floating_point *A, int n, int m, const char *tp) |
| Triangular solve (right hand side). BLAS: ?trsm. | |
| void | symmetricrankupdate (floating_point *P, const floating_point *E, int n, int m) |
| Symmetric rank update. P = P - E*E'. | |
| int | udu (const floating_point *A, floating_point *U, floating_point *d, const int n) |
| UDU decomposition of a symmetrical n x n matrix so that A = U*D*U'. | |
| int | lsame_ (const char *a, const char *b) |
| int | strsm_ (const char *side, const char *uplo, const char *transa, const char *diag, int *m, int *n, floating_point *alpha, const floating_point *a, int *lda, floating_point *b, int *ldb) |
| int | sgemm_ (char *transa, char *transb, int *m, int *n, int *k, floating_point *alpha, floating_point *a, int *lda, floating_point *b, int *ldb, floating_point *beta, floating_point *c__, int *ldc) |
| int | ssyrk_ (char *uplo, char *trans, int *n, int *k, floating_point *alpha, floating_point *a, int *lda, floating_point *beta, floating_point *c__, int *ldc) |
| int | ssymm_ (char *side, char *uplo, int *m, int *n, floating_point *alpha, floating_point *a, int *lda, floating_point *b, int *ldb, floating_point *beta, floating_point *c__, int *ldc) |
| int | strmm_ (const char *side, const char *uplo, const char *transa, const char *diag, int *m, int *n, floating_point *alpha, floating_point *a, int *lda, floating_point *b, int *ldb) |
| const uint32 | POINTING_MEASUREMENT_MEASUREMENT_VECTOR_ELEMENTS (3) |
| const uint32 | POINTING_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS (3) |
| const uint32 | POINTING_MEASUREMENT_MEASUREMENT_NOISE_ELEMENTS (3) |
| int16 | calcMachNumber (const clockwerk::CartesianVector< 3 > &v_veh__wind, floating_point speed_of_sound, floating_point &mach) |
| Calculate Mach number from velocity and speed of sound. | |
| int16 | calcAngleOfAttack (const clockwerk::CartesianVector< 3 > &v_veh__wind, floating_point &angle_of_attack) |
| Calculate angle of attack from alpha = atan(vz/vx). | |
| int16 | calcSideslipAngle (const clockwerk::CartesianVector< 3 > &v_veh__wind, floating_point &sideslip) |
| Calculate sideslip angle from beta = asin(vy/v_inf). | |
| int16 | pcrToDeticLla (clockwerk::CartesianVector< 3 > pos__pcr, floating_point r_planet_equatorial, floating_point flattening, floating_point &lat_rad, floating_point &lon_rad, floating_point &alt) |
| Function to calculate detic LLA from PCR state. | |
| int16 | llaDeticToPCR (double r_planet_equitorial, double flattening, double lat_rad, double lon_rad, double alt, clockwerk::CartesianVector< 3 > &pos__pcr) |
| Function to return the PCR (planet centered rotating) position of an object from detic latitude, longitude, and altitude. | |
| int16 | llaDeticToPCR (float r_planet_equitorial, float flattening, float lat_rad, float lon_rad, float alt, clockwerk::CartesianVector< 3 > &pos__pcr) |
| clockwerk::DCM | enuFromLatLonDetic (floating_point lat_rad, floating_point lon_rad) |
| Function to calculate the East, North, Up frame attitude from detic lat and lon. | |
| clockwerk::DCM | nedFromLatLonDetic (floating_point lat_rad, floating_point lon_rad) |
| Function to calculate the North, East, Down frame attitude from detic lat and lon. | |
| int16 | nedToLlaDetic (floating_point lat_origin_deg, floating_point lon_origin_deg, floating_point alt_origin, clockwerk::CartesianVector< 3 > pos__ned, floating_point r_planet_equatorial, floating_point flattening, floating_point &lat_deg, floating_point &lon_deg, floating_point &alt) |
| Function to calculate detic LLA from NED position. | |
Variables | |
| const clockwerk::CartesianVector< 3 > | ZERO_VECTOR = clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}) |
| Zero vector. | |
| const floating_point | NSEC_TO_BIT32 = (0xFFFFFFFF) / clockwerk::NSEC_PER_SEC_I |
| Factor to convert nanoseconds to 2^-32 seconds. | |
| PlanetDefaults | earth_wgs84 |
| PlanetDefaults | moon_nasa |
| PlanetDefaults | sun_nasa |
| PlanetDefaults | mercury_nasa |
| PlanetDefaults | venus_nasa |
| PlanetDefaults | mars_nasa |
| PlanetDefaults | jupiter_nasa |
| PlanetDefaults | uranus_nasa |
| PlanetDefaults | neptune_nasa |
| std::array< PlanetDefaults *, 9 > | defined_planet_defaults = {&earth_wgs84, &moon_nasa, &sun_nasa, &mercury_nasa, &venus_nasa, &mars_nasa, &jupiter_nasa, &uranus_nasa, &neptune_nasa} |
| A single container to hold all defaults for searchability. | |
| const floating_point | TWO_PI = 2.0*M_PI |
| const floating_point | DEGREES_TO_RADIANS = M_PI/180.0 |
| const floating_point | RADIANS_TO_DEGREES = 180.0/M_PI |
| const floating_point | SPEED_OF_LIGHT = 299792458.0 |
| const floating_point | BOLTZMANN_CONSTANT = 1.380649e-23 |
| const floating_point | AU_TO_METERS = 149597870700.0 |
| const floating_point | METERS_TO_AU = 1.0/149597870700.0 |
| const floating_point | KM_TO_METERS = 1000.0 |
| const floating_point | METERS_TO_KM = 0.001 |
| const floating_point | METERS_TO_FEET = 3.28084 |
| const floating_point | FEET_TO_METERS = 1.0/METERS_TO_FEET |
| const floating_point | METERS_TO_INCHES = METERS_TO_FEET*12.0 |
| const floating_point | INCHES_TO_METERS = 1.0/METERS_TO_INCHES |
| const double | METERS_TO_NAUTICAL_MILES = 1.0/1852.0 |
| const double | LBS_TO_KG = 0.45359237 |
| const double | KG_TO_LBS = 1.0 / LBS_TO_KG |
| const double | POUND_FORCE_INCH_SECOND2_TO_METER2_KG = 0.1129848 |
| const floating_point | SECONDS_TO_MILLISECONDS = 1000.0 |
| const floating_point | SECONDS_TO_NANOSECONDS = 1000000000.0 |
| const floating_point | NANOSECONDS_TO_SECONDS = 1.0 / SECONDS_TO_NANOSECONDS |
| const floating_point | MILLISECONDS_TO_NANOSECONDS = 1000000.0 |
| const floating_point | NANOSECONDS_TO_MILLISECONDS = 1.0 / MILLISECONDS_TO_NANOSECONDS |
| const floating_point | SECONDS_TO_MICROSECONDS = 1000000.0 |
| const floating_point | MICROSECONDS_TO_SECONDS = 1.0 / SECONDS_TO_MICROSECONDS |
| const floating_point | MICROSECONDS_TO_NANOSECONDS = 1000.0 |
| const floating_point | MICROSECONDS_TO_MILLISECONDS = 1.0/1000.0 |
| const floating_point | MINUTES_TO_SECONDS = 60.0 |
| const floating_point | SECONDS_TO_MINUTES = 1.0 / MINUTES_TO_SECONDS |
| const floating_point | HOURS_TO_SECONDS = MINUTES_TO_SECONDS*MINUTES_TO_SECONDS |
| const floating_point | DAYS_TO_SECONDS = 24.0*HOURS_TO_SECONDS |
| const floating_point | YEARS_TO_SECONDS = 365.25*DAYS_TO_SECONDS |
| const floating_point | PSI_TO_PASCAL = 6894.7572932 |
| const floating_point | PASCAL_TO_PSI = 1.0/PSI_TO_PASCAL |
| const floating_point | MILLIBAR_TO_PASCAL = 100.0 |
| const floating_point | PASCAL_TO_MILLIBAR = 1.0/MILLIBAR_TO_PASCAL |
| const uint8 | PRINT_SIZE = 200 |
| const uint8 | TIME_STR_SIZE = 20 |
| Constant for time string conversion. | |
| const uint8 | FILE_NAME_MAX = 100 |
| Constant for filenaming. | |
| const uint8 | LENGTH_TERM_BYTES = 4 |
| const uint8 | TERM_BYTES [LENGTH_TERM_BYTES] = {0xBA, 0xDD, 0xAD, 0xDA} |
| const floating_point | MINIMUM_COV_DIAG_EPSILON = 1e-6 |
| const uint32 | INERTIAL_NAVIGATION_STATE_VECTOR_ELEMENTS = 16 |
| const uint32 | INERTIAL_NAV_RATES_VECTOR_SIZE = 16 + 16*16 |
| char | CHAR_N = 'N' |
| char | CHAR_U = 'U' |
| char | CHAR_R = 'R' |
| char | CHAR_L = 'L' |
| char | CHAR_T = 'T' |
| enum warpos::file_descriptors_e : uint8 |
Options when opening files.
| enum warpos::tlm_priority_e : uint8 |
| int16 warpos::calcAngleOfAttack | ( | const clockwerk::CartesianVector< 3 > & | v_veh__wind, |
| floating_point & | angle_of_attack ) |
Calculate angle of attack from alpha = atan(vz/vx).
| v_veh__wind | Velocity vector for the vehicle relative to the air |
| angle_of_attack | Implicit return of the angle of attack, in radians |
| int16 warpos::calcMachNumber | ( | const clockwerk::CartesianVector< 3 > & | v_veh__wind, |
| floating_point | speed_of_sound, | ||
| floating_point & | mach ) |
Calculate Mach number from velocity and speed of sound.
| v_veh__wind | Velocity vector for the vehicle relative to the air |
| speed_of_sound | The speed of sound |
| mach | Implicit return of the mach number |
| int16 warpos::calcSideslipAngle | ( | const clockwerk::CartesianVector< 3 > & | v_veh__wind, |
| floating_point & | sideslip ) |
Calculate sideslip angle from beta = asin(vy/v_inf).
| v_veh__wind | Velocity vector for the vehicle relative to the air |
| sideslip | Implicit return of the sideslip angle, in radians |
| std::array< T, N > warpos::changeEndian | ( | const std::array< T, N > & | in | ) |
Specialized version of changeEndian which manages array values correctly.
| T warpos::changeEndian | ( | T | in | ) |
Change the endianness of a number.
| in | The number to change endianness of |
| int warpos::cholesky | ( | floating_point * | A, |
| const int | n, | ||
| int | onlyWriteLowerPart ) |
Calculate the lower triangular matrix L, so that L*L' = A. Operation count: n^3/6 with n square roots. BLAS equivalent: ?potrf.
Optmized for
| [in,out] | A | Symmetric, positive definite (n x n) matrix. Only upper triangular part needs to be given. Lower triangular part is overwritten with L. |
| [in] | n | Dimension of A |
| [in] | onlyWriteLowerPart | If set to 0, overwrite the upper triangular part with zeros. Set to e.g. -1 to leave the upper triangular part untouched. |
| int16 warpos::computeCRC16 | ( | uint32 & | crc, |
| const uint8 * | packet_ptr, | ||
| size_t | length, | ||
| uint32 | input_crc = 0 ) |
Function to compute the CRC16 checksum for a packet.
| crc | CRC16 checksum PBR |
| packet_ptr | Pointer to the packet |
| length | Length of the packet in bytes |
| input_crc | Starting/seed value for use in the CRC calculation |
| int16 warpos::computeFletcher8Checksum | ( | const uint8 * | packet_ptr, |
| uint8 | length, | ||
| uint16 & | checksum ) |
Function to calculate the fletcher 8 checksum.
| packet_ptr | Pointer to the packet |
| length | Length of the packet in bytes |
| checksum | Reference to checksum to load data to |
| void warpos::convertImuInertialCg | ( | const clockwerk::CartesianVector< 3 > & | accel_meas_imu_I__imu, |
| const clockwerk::CartesianVector< 3 > & | ang_vel_meas_imu_I__imu, | ||
| const clockwerk::CartesianVector< 3 > & | ang_accel_meas_imu_I__imu, | ||
| const clockwerk::CartesianVector< 3 > & | lever_arm_imu_body__body, | ||
| const clockwerk::Quaternion & | quat_imu_body, | ||
| const clockwerk::CartesianVector< 3 > & | accel_bias__imu, | ||
| const clockwerk::CartesianVector< 3 > & | ang_vel_bias__imu, | ||
| clockwerk::CartesianVector< 3 > & | accel_body_I__body, | ||
| clockwerk::CartesianVector< 3 > & | ang_vel_body_I__body ) |
Rotate IMU measurement from IMU frame to inertial body representation.
| accel_meas_imu_I__imu | The acceleration measured by the IMU |
| ang_vel_meas_imu_I__imu | The angular velocity measured by the IMU |
| ang_accel_meas_imu_I__imu | The angular acceleration measured by the IMU, often derived from differencing ang vel. |
| lever_arm_imu_body__body | The lever arm from the body CG to the IMU frame, represented in body coords |
| quat_imu_body | The quaternion describing the orientation of the IMU frame wrt the body frame |
| accel_bias__imu | The IMU acceleration bias as measured in the imu frame |
| ang_vel_bias__imu | The angular velocity bias as measured in the imu frame |
| accel_body_I__body | Implicit return of inertial acceleration as measured in body frame |
| ang_vel_body_I__body | Implicit return of inertial angular velocity as measured in body frame |
| accel_meas__imu | The acceleration measured by the IMU |
| ang_vel_meas__imu | The angular velocity measured by the IMU |
| ang_accel_meas__imu | The angular acceleration measured by the IMU, often derived from differencing ang vel. |
| lever_arm__body | The lever arm from the body CG to the IMU frame, represented in body coords |
| quat_imu_body | The quaternion describing the orientation of the IMU frame wrt the body frame |
| accel_bias__imu | The IMU acceleration bias as measured in the imu frame |
| ang_vel_bias__imu | The angular velocity bias as measured in the imu frame |
| accel_inrtl__body | Implicit return of inertial acceleration as measured in body frame |
| ang_vel_inrtl__body | Implicit return of inertial angular velocity as measured in body frame |
| int warpos::decorrelate | ( | floating_point * | z, |
| floating_point * | Ht, | ||
| floating_point * | R, | ||
| int | n, | ||
| int | m ) |
Decorrelate measurements. For a given covariance matrix R of correlated measurements, calculate a vector of decorrelated measurements (and the matching H-matrix) so that the new covariance R of the decorrelated measurements is an identity matrix.
| [in,out] | z | Vector of correlated measurements (m x 1). |
| [in,out] | Ht | Transposed measurement sensitivity matrix / design matrix so that z = Ht'*x (n x m). |
| [in,out] | R | Measurement covariance matrix, replaced in-place by chol(R) (m x m). So the input is R, overwritten with L such that L*L'=R. This is useful for the decorrelation of other measurements. |
| [in] | n | Number of columns in H (for a Kalman filter: length of state vector x). |
| [in] | m | Number of measurements in z. |
Note: If R only has diagonal elements, the only use would be that an identity covariance matrix R is required, otherwise a call to this function is not required.
A typical use case would be the decorrelation of meassurements for the UDU filter:
decorrelate(z, Ht, R, n, m); // in-place decorrelation of z and Ht floating_point Reye[3*3]; mateye(Reye, 3); // set R to eye(2) kalman_udu(x, U, d, z, Reye, Ht, n, m, 0.0f, 0);
If Ht and R do not change, subsequently only measurements z need to be decorrelated. As the input R is replaced by L (such that L*L' = R), L can be reused to decorrelate further measurements:
trisolve(L, z, m, 1, N_CONST);
| clockwerk::DCM warpos::enuFromLatLonDetic | ( | floating_point | lat_rad, |
| floating_point | lon_rad ) |
Function to calculate the East, North, Up frame attitude from detic lat and lon.
| lat_rad | The detic latitude in radians |
| lon_rad | The longitude in radians |
| warpos::EXCLUDE_FROM_COVERAGE | ( | const FlightExecutive &FlightExecutive::operator | = (const FlightExecutive& original) { return *this; } | ) |
| const uint32 warpos::GPS_MEASUREMENT_MEASUREMENT_NOISE_ELEMENTS | ( | 3 | ) |
| const uint32 warpos::GPS_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS | ( | 4 | ) |
| const uint32 warpos::GPS_MEASUREMENT_VECTOR_ELEMENTS | ( | 3 | ) |
| void warpos::kalman_predict | ( | floating_point * | x, |
| floating_point * | P, | ||
| const floating_point * | Phi, | ||
| const floating_point * | G, | ||
| const floating_point * | Q, | ||
| int | n, | ||
| int | r ) |
Kalman Temporal / Prediction Step.
| [in,out] | x | (optional *) state vector with size (n x 1) |
| [in,out] | P | covariance matrix of state estimation uncertainty P (n x n). |
| [in] | Phi | state transition matrix (n x n) |
| [in] | G | process noise distribution matrix (n x r) |
| [in] | Q | diagonal vector of covariance matrix of process noise in the stochastic system model (r x 1) (diag(Q) has size r x r) |
| [in] | n | State vector size n. |
| [in] | r | Process noise matrix size. |
(*) Optional, as a non-linear filter will do the prediction of the state vector with a dedicated (non-linear) function. This will basically just predict the state vector with:
x^{-} = Phi*x^{+}
P^{+} = Phi*P^{-}*Phi' + G*diag(Q)*G'
| int warpos::kalman_takasu | ( | floating_point * | x, |
| floating_point * | P, | ||
| const floating_point * | dz, | ||
| const floating_point * | R, | ||
| const floating_point * | Ht, | ||
| int | n, | ||
| int | m, | ||
| floating_point | chi2_threshold, | ||
| floating_point * | chi2 ) |
Kalman Filter Routine (Takasu Formulation).
The Takasu formulation typically results in an efficient update step which saves CPU time. Numerical stability is good (but not as good as Joseph's form update or the UDU update). This is a good choice if speed is important, the problem formulation is well conditioned numerically and the UDU "square root" formulation is too invasive for the given environment.
| [in,out] | x | System state (n x 1) |
| [in,out] | P | Upper triangular Covariance matrix of state estimation uncertainty (n x n) |
| [in] | dz | Measurement residual vector: measurement vs. expected measurement: z - H*x (m x 1) |
| [in] | R | Full covariance matrix of measurement uncertainty (m x m) |
| [in] | Ht | Transposed (!) measurement sensitivity matrix (n x m) (H would be m x n) |
| [in] | n | Number of state variables |
| [in] | m | Number of measurements |
| [in] | chi2_threshold | Scalar threshold for chi2 outlier removal threshold. Set to 0.0f to disable the outlier removal. |
| [out] | chi2 | Calculate the chi2 test statistics |
Note 1: only the upper triangular part of P is referenced and updated. Note 2: chi2 threshold for 95% confidence is given in table below. E.g. if dz contains 3 measurements, use 7.8147 (MATLAB chi2inv(0.95, 3)).
chi2inv(0.95, 1:20): chi95% = [ 3.8415 5.9915 7.8147 9.4877 11.0705 12.5916 14.0671 ... 15.5073 16.9190 18.3070 19.6751 21.0261 22.3620 23.6848 ... 24.9958 26.2962 27.5871 28.8693 30.1435 31.4104 ];
chi2inv(0.99, 1:20): chi99% = [ 6.6349, 9.2103, 11.3449, 13.2767, 15.0863, 16.8119, ... 18.4753, 20.0902, 21.6660, 23.2093, 24.7250, 26.2170, 27.6882, ... 29.1412, 30.5779, 31.9999, 33.4087, 34.8053, 36.1909, 37.5662 ];
| int warpos::kalman_udu | ( | floating_point * | x, |
| floating_point * | U, | ||
| floating_point * | d, | ||
| const floating_point * | z, | ||
| const floating_point * | R, | ||
| const floating_point * | Ht, | ||
| int | n, | ||
| int | m, | ||
| floating_point | chi2_threshold, | ||
| int | downweight_outlier ) |
(Robust) Square Root Kalman Filter (Bierman) update routine for linear systems.
| [in,out] | x | System state (n x 1) |
| [in,out] | U | Unit upper triangular factor of covariance matrix of a priori state uncertainty (n x n) |
| [in,out] | d | Unit upper triangular factor of covariance matrix of a priori state uncertainty (n x 1) |
| [in] | z | Measurement vector: z = H*x (m x 1) |
| [in] | R | Full covariance matrix of measurement uncertainty (m x m) |
| [in] | Ht | Transposed (!) measurement sensitivity matrix (n x m) (H would be m x n) |
| [in] | n | Number of state variables |
| [in] | m | Number of measurements |
| [in] | chi2_threshold | Scalar threshold for outlier classification. Set to 0.0f to disable. |
| [in] | downweight_outlier | If set to 0, measurements classified as outliers are skipped. |
| void warpos::kalman_udu_predict | ( | floating_point * | x, |
| floating_point * | U, | ||
| floating_point * | d, | ||
| const floating_point * | Phi, | ||
| const floating_point * | G, | ||
| const floating_point * | Q, | ||
| int | n, | ||
| int | r ) |
UDU' (Thornton) Filter Temporal / Prediction Step.
Catherine Thornton's modified weighted Gram-Schmidt orthogonalization method for the predictor update of the U-D factors of the covariance matrix of estimation uncertainty in Kalman filtering. Source: [1].
P = U*D*U' = Uin * diag(din) * Uin'
| [in,out] | x | (optional *) state vector with size (n x 1) |
| [in,out] | U | unit upper triangular factor (U) of the modified Cholesky factors (U-D factors) of the covariance matrix of corrected state estimation uncertainty P^{+} (n x n). Updated in-place to the modified factors (U-D) of the covariance matrix of predicted state estimation uncertainty P^{-}, so that U*diag(d)*U' = P^{-} after this function. |
| [in,out] | d | diagonal factor (d) vector (n x 1) of the U-D factors of the covariance matrix of corrected estimation uncertainty P^{+}, so that diag(d) = D. Updated in-place so that P^{-} = U*diag(d)*U' |
| [in] | Phi | state transition matrix (n x n) |
| [in,out] | G | process noise distribution matrix (modified, if necessary to make the associated process noise covariance diagonal) (n x r) |
| [in] | Q | diagonal vector of covariance matrix of process noise in the stochastic system model (r x 1) (diag(Q) has size r x r) |
| [in] | n | State vector size n. |
| [in] | r | Process noise matrix size. |
(*) Optional, as a non-linear filter will do the prediction of the state vector with a dedicated (non-linear) function. This will basically just predict the state vector with:
x^{-} = Phi*x^{+}
P^{+} = Phi*P^{-}*Phi' + G*diag(Q)*G'
References: [1] Grewal, Weill, Andrews. "Global positioning systems, inertial navigation, and integration". 1st ed. John Wiley & Sons, New York, 2001.
| int warpos::kalman_udu_scalar | ( | floating_point * | x, |
| floating_point * | U, | ||
| floating_point * | d, | ||
| const floating_point | dz, | ||
| const floating_point | R, | ||
| const floating_point * | H_line, | ||
| int | n ) |
Square Root Kalman Filter (Bierman) Routine for a single scalar measurement.
| [in,out] | x | System state (n x 1) |
| [in,out] | U | Unit upper triangular factor of covariance matrix of a priori state uncertainty (n x n) |
| [in,out] | d | Unit upper triangular factor of covariance matrix of a priori state uncertainty (n x 1) |
| [in] | dz | Measurement residual dz = z - H*x (1 x 1). |
| [in] | R | Scalar covariance of measurement uncertainty (1 x 1) |
| [in] | H_line | Row of measurement sensitivity matrix (n x 1) |
| [in] | n | Number of state variables |
| int16 warpos::llaDeticToPCR | ( | double | r_planet_equitorial, |
| double | flattening, | ||
| double | lat_rad, | ||
| double | lon_rad, | ||
| double | alt, | ||
| clockwerk::CartesianVector< 3 > & | pos__pcr ) |
Function to return the PCR (planet centered rotating) position of an object from detic latitude, longitude, and altitude.
| r_planet_equitorial | The equatorial radius of the planet |
| flattening | Flattening coefficient of the planet |
| lat_rad | The detic latitude in radians |
| lon_rad | The longitude in radians |
| alt | The detic altitude in the same units as the radius (r_planet) |
| pos__pcr | Implicit return of the position of the object in PCR coordinates |
| int16 warpos::llaDeticToPCR | ( | float | r_planet_equitorial, |
| float | flattening, | ||
| float | lat_rad, | ||
| float | lon_rad, | ||
| float | alt, | ||
| clockwerk::CartesianVector< 3 > & | pos__pcr ) |
| int warpos::lsame_ | ( | const char * | a, |
| const char * | b ) |
| void warpos::mateye | ( | floating_point * | A, |
| int | n ) |
Fill array with an identity matrix.
| [out] | A | To be filled (n x n). |
| [in] | n | Dimension of A. |
| void warpos::matmul | ( | const char * | ta, |
| const char * | tb, | ||
| int | n, | ||
| int | k, | ||
| int | m, | ||
| floating_point | alpha, | ||
| const floating_point * | A, | ||
| const floating_point * | B, | ||
| floating_point | beta, | ||
| floating_point * | C ) |
| void warpos::matmulsym | ( | const floating_point * | A_sym, |
| const floating_point * | B, | ||
| int | n, | ||
| int | m, | ||
| floating_point * | C ) |
Multiplication of symmetric matrix A with B: C = A*B.
| [in] | A_sym | (n x n) matrix, only upper triangular part is referenced. |
| [in] | B | (n x m) matrix |
| [in] | n | Rows/columns of A, rows of B |
| [in] | m | columns of B and C |
| [out] | C | (n x m) matrix output of the product A*B |
| clockwerk::DCM warpos::nedFromLatLonDetic | ( | floating_point | lat_rad, |
| floating_point | lon_rad ) |
Function to calculate the North, East, Down frame attitude from detic lat and lon.
| lat_rad | The detic latitude in radians |
| lon_rad | The longitude in radians |
| int16 warpos::nedToLlaDetic | ( | floating_point | lat_origin_deg, |
| floating_point | lon_origin_deg, | ||
| floating_point | alt_origin, | ||
| clockwerk::CartesianVector< 3 > | pos__ned, | ||
| floating_point | r_planet_equatorial, | ||
| floating_point | flattening, | ||
| floating_point & | lat_deg, | ||
| floating_point & | lon_deg, | ||
| floating_point & | alt ) |
Function to calculate detic LLA from NED position.
| lat_origin_deg | Latitude of the NED origin in degrees |
| lon_origin_deg | Longitude of the NED origin in degrees |
| alt_origin | Altitude of the NED origin in same units as r_planet_equatorial |
| pos__ned | The position of the spacecraft in NED frame |
| r_planet_equatorial | The equatorial radius of the planet |
| flattening | The flattening value of the planet |
| lat_deg | Implicit return of latitude in degrees |
| lon_deg | Implicit return of longitude in degrees |
| alt | Implicit return of altitude |
| int16 warpos::pcrToDeticLla | ( | clockwerk::CartesianVector< 3 > | pos__pcr, |
| floating_point | r_planet_equatorial, | ||
| floating_point | flattening, | ||
| floating_point & | lat_rad, | ||
| floating_point & | lon_rad, | ||
| floating_point & | alt ) |
Function to calculate detic LLA from PCR state.
| pos__pcr | The position of the spacecraft in PCR frame |
| r_planet_equatorial | The equatorial radius of the planet |
| flattening | The flattening value of the planet |
| lat_rad | Implicit return of latitude in radians |
| lon_rad | Implicit return of longitude in radians |
| alt | Implicit return of altitude |
| const uint32 warpos::POINTING_MEASUREMENT_MEASUREMENT_NOISE_ELEMENTS | ( | 3 | ) |
| const uint32 warpos::POINTING_MEASUREMENT_MEASUREMENT_VECTOR_ELEMENTS | ( | 3 | ) |
| const uint32 warpos::POINTING_MEASUREMENT_OBSERVER_VECTOR_ELEMENTS | ( | 3 | ) |
| int16 warpos::readCCSDSHeader | ( | uint8 * | header, |
| bool & | is_tlm, | ||
| uint16 & | apid, | ||
| uint8 & | seq_flag, | ||
| uint16 & | seq_count, | ||
| uint16 & | data_len, | ||
| clockwerk::Time & | timestamp, | ||
| uint8 & | instance ) |
Function to read the CCSDS packet header and extract its contents.
| header | Pointer to the head of the buffer that holds the header (must be at least 6 bytes long) |
| is_tlm | Packet type bit PBR |
| apid | Packet APID PBR |
| seq_flag | Sequence flag PBR |
| seq_count | Sequence count PBR |
| data_len | Packet data length PBR |
| timestamp | Time of the packet from secondary header time code PBR |
| instance | APID instance from secondary header PBR |
| int warpos::sgemm_ | ( | char * | transa, |
| char * | transb, | ||
| int * | m, | ||
| int * | n, | ||
| int * | k, | ||
| floating_point * | alpha, | ||
| floating_point * | a, | ||
| int * | lda, | ||
| floating_point * | b, | ||
| int * | ldb, | ||
| floating_point * | beta, | ||
| floating_point * | c__, | ||
| int * | ldc ) |
| int warpos::ssymm_ | ( | char * | side, |
| char * | uplo, | ||
| int * | m, | ||
| int * | n, | ||
| floating_point * | alpha, | ||
| floating_point * | a, | ||
| int * | lda, | ||
| floating_point * | b, | ||
| int * | ldb, | ||
| floating_point * | beta, | ||
| floating_point * | c__, | ||
| int * | ldc ) |
| int warpos::ssyrk_ | ( | char * | uplo, |
| char * | trans, | ||
| int * | n, | ||
| int * | k, | ||
| floating_point * | alpha, | ||
| floating_point * | a, | ||
| int * | lda, | ||
| floating_point * | beta, | ||
| floating_point * | c__, | ||
| int * | ldc ) |
| int warpos::strmm_ | ( | const char * | side, |
| const char * | uplo, | ||
| const char * | transa, | ||
| const char * | diag, | ||
| int * | m, | ||
| int * | n, | ||
| floating_point * | alpha, | ||
| floating_point * | a, | ||
| int * | lda, | ||
| floating_point * | b, | ||
| int * | ldb ) |
| int warpos::strsm_ | ( | const char * | side, |
| const char * | uplo, | ||
| const char * | transa, | ||
| const char * | diag, | ||
| int * | m, | ||
| int * | n, | ||
| floating_point * | alpha, | ||
| const floating_point * | a, | ||
| int * | lda, | ||
| floating_point * | b, | ||
| int * | ldb ) |
| void warpos::symmetricrankupdate | ( | floating_point * | P, |
| const floating_point * | E, | ||
| int | n, | ||
| int | m ) |
Symmetric rank update. P = P - E*E'.
| [in,out] | P | Matrix (n x n) to be updated (only upper part is referenced and updated) |
| [in] | E | Matrix (n x m) including the update |
| [in] | n | Number of rows and cols in P, rows in E |
| [in] | m | Number of cols in E |
| void warpos::trisolve | ( | const floating_point * | A, |
| floating_point * | B, | ||
| int | n, | ||
| int | m, | ||
| const char * | tp ) |
Triangular solve BLAS: ?trsm.
Solve matrix equation: A*X = B
| [in] | A | Given lower triangular matrix (dimension n x n) |
| [in,out] | B | Matrix being overwritten by X (dimension n x m) |
| [in] | n | Matrix dimension (rows / columns of A) |
| [in] | m | Matrix dimension (cols of B) |
| [in] | tp | Transpose L? |
| void warpos::trisolveright | ( | const floating_point * | L, |
| floating_point * | A, | ||
| int | n, | ||
| int | m, | ||
| const char * | tp ) |
Triangular solve (right hand side). BLAS: ?trsm.
Solve matrix equation: X*L = A
| [in] | L | Given lower triangular matrix (dimension n x n) |
| [in,out] | A | Matrix being overwritten by X (dimension m x n) |
| [in] | n | Matrix dimension (rows / columns of L) |
| [in] | m | Matrix dimension (rows of A) |
| [in] | tp | Transpose L? |
| int warpos::udu | ( | const floating_point * | A, |
| floating_point * | U, | ||
| floating_point * | d, | ||
| const int | n ) |
UDU decomposition of a symmetrical n x n matrix so that A = U*D*U'.
D is returned as diagonal vector d so that diag(d) = D. U Is an unit upper triangular matrix U.
| [in] | A | (n x n) input matrix |
| [out] | U | (n x n) Output upper unit triangular matrix U |
| [out] | d | (n x 1) Output vector d (D = diag(d)) |
| [in] | n | Matrix dimension n |
| int16 warpos::validateCRC16 | ( | const uint32 & | crc, |
| const uint8 * | packet_ptr, | ||
| size_t | length, | ||
| uint32 | input_crc = 0 ) |
Function to validate the CRC16 checksum for a packet.
| crc | CRC16 checksum to validate |
| packet_ptr | Pointer to the packet |
| length | Length of the packet in bytes |
| input_crc | Starting/seed value for use in the CRC calculation |
| int16 warpos::validateFletcher8Checksum | ( | const uint8 * | packet_ptr, |
| uint8 | length, | ||
| const uint16 & | checksum ) |
Function to validate the fletcher8 checksum for a packet.
| packet_ptr | Pointer to the packet |
| length | Length of the packet in bytes |
| checksum | The checksum to validate |
| int16 warpos::writeCCSDSHeader | ( | uint8 * | header, |
| bool | is_tlm, | ||
| uint16 | apid, | ||
| uint8 | seq_flag, | ||
| uint16 | seq_count, | ||
| uint16 | data_len, | ||
| clockwerk::Time | timestamp, | ||
| uint8 | instance ) |
Function to write the CCSDS packet header.
| header | Pointer to the head of the buffer that will hold the header (must be at least 12 bytes long for both primary and secondary headers) |
| is_tlm | Whether or not this is a telemetry packet, for packet type bit |
| apid | Packet APID |
| seq_flag | Sequence flag |
| seq_count | Sequence count |
| data_len | Packet data length in bytes (not length - 1 as specified by CCSDS standard; this function will subtract 1) |
| timestamp | Time of the packet for secondary header time code |
| instance | APID instance for secondary header |
| const floating_point warpos::AU_TO_METERS = 149597870700.0 |
| const floating_point warpos::BOLTZMANN_CONSTANT = 1.380649e-23 |
| char warpos::CHAR_L = 'L' |
| char warpos::CHAR_N = 'N' |
| char warpos::CHAR_R = 'R' |
| char warpos::CHAR_T = 'T' |
| char warpos::CHAR_U = 'U' |
| const floating_point warpos::DAYS_TO_SECONDS = 24.0*HOURS_TO_SECONDS |
| std::array< PlanetDefaults *, 9 > warpos::defined_planet_defaults = {&earth_wgs84, &moon_nasa, &sun_nasa, &mercury_nasa, &venus_nasa, &mars_nasa, &jupiter_nasa, &uranus_nasa, &neptune_nasa} |
A single container to hold all defaults for searchability.
| const floating_point warpos::DEGREES_TO_RADIANS = M_PI/180.0 |
| PlanetDefaults warpos::earth_wgs84 |
| const floating_point warpos::FEET_TO_METERS = 1.0/METERS_TO_FEET |
| const uint8 warpos::FILE_NAME_MAX = 100 |
Constant for filenaming.
| const floating_point warpos::HOURS_TO_SECONDS = MINUTES_TO_SECONDS*MINUTES_TO_SECONDS |
| const floating_point warpos::INCHES_TO_METERS = 1.0/METERS_TO_INCHES |
| const uint32 warpos::INERTIAL_NAV_RATES_VECTOR_SIZE = 16 + 16*16 |
| const uint32 warpos::INERTIAL_NAVIGATION_STATE_VECTOR_ELEMENTS = 16 |
| PlanetDefaults warpos::jupiter_nasa |
| const double warpos::KG_TO_LBS = 1.0 / LBS_TO_KG |
| const floating_point warpos::KM_TO_METERS = 1000.0 |
| const double warpos::LBS_TO_KG = 0.45359237 |
| const uint8 warpos::LENGTH_TERM_BYTES = 4 |
| PlanetDefaults warpos::mars_nasa |
| PlanetDefaults warpos::mercury_nasa |
| const floating_point warpos::METERS_TO_AU = 1.0/149597870700.0 |
| const floating_point warpos::METERS_TO_FEET = 3.28084 |
| const floating_point warpos::METERS_TO_INCHES = METERS_TO_FEET*12.0 |
| const floating_point warpos::METERS_TO_KM = 0.001 |
| const double warpos::METERS_TO_NAUTICAL_MILES = 1.0/1852.0 |
| const floating_point warpos::MICROSECONDS_TO_MILLISECONDS = 1.0/1000.0 |
| const floating_point warpos::MICROSECONDS_TO_NANOSECONDS = 1000.0 |
| const floating_point warpos::MICROSECONDS_TO_SECONDS = 1.0 / SECONDS_TO_MICROSECONDS |
| const floating_point warpos::MILLIBAR_TO_PASCAL = 100.0 |
| const floating_point warpos::MILLISECONDS_TO_NANOSECONDS = 1000000.0 |
| const floating_point warpos::MINIMUM_COV_DIAG_EPSILON = 1e-6 |
| const floating_point warpos::MINUTES_TO_SECONDS = 60.0 |
| PlanetDefaults warpos::moon_nasa |
| const floating_point warpos::NANOSECONDS_TO_MILLISECONDS = 1.0 / MILLISECONDS_TO_NANOSECONDS |
| const floating_point warpos::NANOSECONDS_TO_SECONDS = 1.0 / SECONDS_TO_NANOSECONDS |
| PlanetDefaults warpos::neptune_nasa |
| const floating_point warpos::NSEC_TO_BIT32 = (0xFFFFFFFF) / clockwerk::NSEC_PER_SEC_I |
Factor to convert nanoseconds to 2^-32 seconds.
| const floating_point warpos::PASCAL_TO_MILLIBAR = 1.0/MILLIBAR_TO_PASCAL |
| const floating_point warpos::PASCAL_TO_PSI = 1.0/PSI_TO_PASCAL |
| const double warpos::POUND_FORCE_INCH_SECOND2_TO_METER2_KG = 0.1129848 |
| const uint8 warpos::PRINT_SIZE = 200 |
| const floating_point warpos::PSI_TO_PASCAL = 6894.7572932 |
| const floating_point warpos::RADIANS_TO_DEGREES = 180.0/M_PI |
| const floating_point warpos::SECONDS_TO_MICROSECONDS = 1000000.0 |
| const floating_point warpos::SECONDS_TO_MILLISECONDS = 1000.0 |
| const floating_point warpos::SECONDS_TO_MINUTES = 1.0 / MINUTES_TO_SECONDS |
| const floating_point warpos::SECONDS_TO_NANOSECONDS = 1000000000.0 |
| const floating_point warpos::SPEED_OF_LIGHT = 299792458.0 |
| PlanetDefaults warpos::sun_nasa |
| const uint8 warpos::TERM_BYTES[LENGTH_TERM_BYTES] = {0xBA, 0xDD, 0xAD, 0xDA} |
| const uint8 warpos::TIME_STR_SIZE = 20 |
Constant for time string conversion.
| const floating_point warpos::TWO_PI = 2.0*M_PI |
| PlanetDefaults warpos::uranus_nasa |
| PlanetDefaults warpos::venus_nasa |
| const floating_point warpos::YEARS_TO_SECONDS = 365.25*DAYS_TO_SECONDS |
| const clockwerk::CartesianVector< 3 > warpos::ZERO_VECTOR = clockwerk::CartesianVector<3>({0.0, 0.0, 0.0}) |
Zero vector.