/**************************************************************************** * * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. * 3. Neither the name ECL nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ /** * @file estimator_base.h * Definition of base class for attitude estimators * * @author Roman Bast * @author Siddharth Bharat Purohit * */ namespace estimator { struct gps_message { uint64_t time_usec; int32_t lat; // Latitude in 1E-7 degrees int32_t lon; // Longitude in 1E-7 degrees int32_t alt; // Altitude in 1E-3 meters (millimeters) above MSL uint8_t fix_type; // 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time float eph; // GPS horizontal position accuracy in m float epv; // GPS vertical position accuracy in m float sacc; // GPS speed accuracy in m/s uint64_t time_usec_vel; // Timestamp for velocity informations float vel_m_s; // GPS ground speed (m/s) float vel_ned[3]; // GPS ground speed NED bool vel_ned_valid; // GPS ground speed is valid uint8_t nsats; // number of satellites used float gdop; // geometric dilution of precision }; typedef matrix::Vector Vector2f; typedef matrix::Vector Vector3f; typedef matrix::Quaternion Quaternion; typedef matrix::Matrix Matrix3f; struct outputSample { Quaternion quat_nominal; Vector3f vel; Vector3f pos; uint64_t time_us; }; struct imuSample { Vector3f delta_ang; Vector3f delta_vel; float delta_ang_dt; float delta_vel_dt; uint64_t time_us; }; struct gpsSample { Vector2f pos; float hgt; Vector3f vel; uint64_t time_us; }; struct magSample { Vector3f mag; uint64_t time_us; }; struct baroSample { float hgt; uint64_t time_us; }; struct rangeSample { float rng; uint64_t time_us; }; struct airspeedSample { float airspeed; uint64_t time_us; }; struct flowSample { Vector2f flowRadXY; Vector2f flowRadXYcomp; uint64_t time_us; }; struct parameters { float mag_delay_ms = 0.0f; float baro_delay_ms = 0.0f; float gps_delay_ms = 200.0f; float airspeed_delay_ms = 200.0f; // input noise float gyro_noise = 0.001f; float accel_noise = 0.1f; // process noise float gyro_bias_p_noise = 1e-5f; float accel_bias_p_noise = 1e-3f; float gyro_scale_p_noise = 1e-4f; float mag_p_noise = 1e-2f; float wind_vel_p_noise = 0.05f; float gps_vel_noise = 0.05f; float gps_pos_noise = 1.0f; float baro_noise = 0.1f; float baro_innov_gate = 5.0f; // barometric height innovation consistency gate size in standard deviations float posNE_innov_gate = 5.0f; // GPS horizontal position innovation consistency gate size in standard deviations float vel_innov_gate = 3.0f; // GPS velocity innovation consistency gate size in standard deviations float mag_heading_noise = 3e-2f; // measurement noise used for simple heading fusion float mag_declination_deg = 0.0f; // magnetic declination in degrees float heading_innov_gate = 3.0f; // heading fusion innovation consistency gate size in standard deviations float mag_innov_gate = 3.0f; // magnetometer fusion innovation consistency gate size in standard deviations // these parameters control the strictness of GPS quality checks used to determine uf the GPS is // good enough to set a local origin and commence aiding int gps_check_mask = 21; // bitmask used to control which GPS quality checks are used float req_hacc = 5.0f; // maximum acceptable horizontal position error float req_vacc = 8.0f; // maximum acceptable vertical position error float req_sacc = 1.0f; // maximum acceptable speed error int req_nsats = 6; // minimum acceptable satellite count float req_gdop = 2.0f; // maximum acceptable geometric dilution of precision float req_hdrift = 0.3f; // maximum acceptable horizontal drift speed float req_vdrift = 0.5f; // maximum acceptable vertical drift speed }; struct stateSample { Vector3f ang_error; Vector3f vel; Vector3f pos; Vector3f gyro_bias; Vector3f gyro_scale; float accel_z_bias; Vector3f mag_I; Vector3f mag_B; Vector2f wind_vel; Quaternion quat_nominal; }; struct fault_status_t { bool bad_mag_x: 1; bool bad_mag_y: 1; bool bad_mag_z: 1; bool bad_airspeed: 1; bool bad_sideslip: 1; }; // publish the status of various GPS quality checks union gps_check_fail_status_u { struct { uint16_t fix : 1; // 0 - true if the fix type is insufficient (no 3D solution) uint16_t nsats : 1; // 1 - true if number of satellites used is insufficient uint16_t gdop : 1; // 2 - true if geometric dilution of precision is insufficient uint16_t hacc : 1; // 3 - true if reported horizontal accuracy is insufficient uint16_t vacc : 1; // 4 - true if reported vertical accuracy is insufficient uint16_t sacc : 1; // 5 - true if reported speed accuracy is insufficient uint16_t hdrift : 1; // 6 - true if horizontal drift is excessive (can only be used when stationary on ground) uint16_t vdrift : 1; // 7 - true if vertical drift is excessive (can only be used when stationary on ground) uint16_t hspeed : 1; // 8 - true if horizontal speed is excessive (can only be used when stationary on ground) uint16_t vspeed : 1; // 9 - true if vertical speed error is excessive } flags; uint16_t value; }; // bitmask containing filter control status union filter_control_status_u { struct { uint8_t angle_align : 1; // 0 - true if the filter angular alignment is complete uint8_t gps : 1; // 1 - true if GPS measurements are being fused uint8_t opt_flow : 1; // 2 - true if optical flow measurements are being fused uint8_t mag_hdg : 1; // 3 - true if a simple magnetic heading is being fused uint8_t mag_3D : 1; // 4 - true if 3-axis magnetometer measurement are being fused uint8_t mag_dec : 1; // 5 - true if synthetic magnetic declination measurements are being fused uint8_t in_air : 1; // 6 - true when the vehicle is airborne uint8_t armed : 1; // 7 - true when the vehicle motors are armed } flags; uint16_t value; }; }