SimpleFlight and state estimation

[JacopoPan]

Is this

In current release we are using ground truth from simulator for our state estimation. We plan to add complimentary filter based state estimation for angular velocity and orientation using 2 sensors (gyroscope, accelerometer) in near future. In more longer term, we plan to integrate another library to do velocity and position estimation using 4 sensors (gyroscope, accelerometer, magnetometer and barometer) using EKF. If you have experience this area than we encourage you to engage with us and contribute!

still the case (I would believe so, by looking at AirSimSimpleFlightEstimator.hpp) and then something that might be worthwhile contributing to?

[madratman]

Yes, that is still the case. And is definitely something worth contributing to via a PR.
If you need help / feedback on the implementation, let's chat on this thread.

[subedisuman]

Hi, I am implementing an EKF-based state estimator in AirSim simple_flight as described in the issue above (using 4 sensors) as part of my Master's Thesis at TUM and IABG. My implementation is still in a very early phase, however, I would much appreciate your feedback. Here is the branch to the forked repository: https://github.com/subedisuman/AirSim/tree/ekf. I am looking forward to your comments and hope my developments will be beneficial for the AirSim community.

[jonyMarino]

Hi @subedisuman! That's great to hear. I will take a look at your fork when having time. I would love to see this becoming a PR.

[subedisuman]

Update

In the following branch, I have implemented an Extended Kalman Filter (EKF) based state estimator as a part of my Master Thesis. It uses the following sensor measurements: IMU, GPS, Barometer, and Magnetometer to estimate the following states: local (NED) x-y-z positions, x-y-z velocities, attitudes, and IMU and barometer sensor biases. I have documented the changes I made below. There is also a demo to try things out and see results in plots :). I have tested it using Linux (Ubuntu 20.04.4 LTS) build of AirSim and UE 4.25. I would say it is content-wise complete and would appreciate your comments and remarks to help make improvements and run tests and validations. I would also like to know the next steps towards a pull request. Thanks :).

Work so far

Files Added

• AirSimSimpleEkf.hpp: contains the main Extended Kalman Filter (EKF) logic that uses IMU measurements to predict the states and GPS, barometer, and magnetometer measurements to update the states.
• AirSimSimpleEkfBase.hpp: contains common getters and setters for the AirSimSimpleEkf class.
• AirSimSimpleEkfModel.hpp: contains mathematical models of the aircraft kinematics and measurement models for sensors.
• AirSimSimpleEkfParams.hpp: contains reading and storage of configurations and parameters required by the EKF.
• IEkf.hpp: contains the pure abstract class interface of EKF.
• EkfStructs.hpp: contains declaration of structs used by EKF in msr/airlib namespace.

Files Modified

Main Modifications

• AirSimSimpleFlightBoard.hpp: sensors pointers are introduced as state variables that are intitialized during construction of the flight board. Additional methods are introduced that gets sensor outputs from the sensor pointers. These methods are used by EKF to get sensor measurements.
• AirSImSimpleFlightEstimator.hpp: added getters for EKF estimated states that are distributed from here to other AirSim components. Can toggle between EKF estimated states and ground truth states.
• AirSimSimpleFlightCommon.hpp: added namespace conversion of structs used by EKF from msr/airlib to simple_flight and vice versa.
• SimpleFlightApi.hpp: added construction of AirSimSimpleEkf and public getters of EKF states.
• Firmware.hpp: added reset and update of EKF.
• OffboardApi.hpp: added minor commlink message to know if EKF states are used throughout (if EKF is enabled?).
• CommonStructs.hpp: added definition of structs used by EKF in simple_flight namespace.
• IBoardSensors.hpp: added interfaces for the sensor board that reads sensor outputs. These are used by EKF.
• IStateEstimator.hpp: added interfaces for getters of EKF states.

Additional Modifications

• AirSimSettings.hpp: added EKF Settings, attached it to vehicle settings, from where EKF Params reads the EKF specific settings added for this implementation.
• GeodeticConverter.hpp: Merged use geodetic converter. And added geodetic2Ned() used in EKF to convert GPS output to NED co-ordinates.
• Environment.hpp: Merged use geodetic converter.
• SensorBase.hpp: added is_new_ flag as a state variable that is set false before a sensor is updated and is set true if a valid output is generated. Also see IMU, GPS, Barometer, and Magnetometer models.
• IMU, Baro, GPS, and Magnetometer sensor models. Addedis_new_ = True whenever an output is produced. Added additional parameters and GPS random white noise. Added IMU turn-on bias as parameters.
• VectorMath.hpp: added additional vector definitions.
• MultirotorApiBase.cpp: removed kinematics.twist.linear.norm() > approx_zero_vel_ check in takeoff().

Unreal

• MultirotorPawnSimApi.cpp: added additional EKF states recording line. Also see MultirotorPawnSimApi.h.

PythonClient

• client.py and RpcLibServerBase.cpp: added resetVehicleApi() that resets a vehicle api. Used to reset EKF of the vehicle.

Requirements

• additional settings:

  {
  "SettingsVersion": 1.2,
  "SimMode": "Multirotor",
  "Recording": {
      "RecordInterval": 0.1,
      "Cameras": []
  },
  "DefaultSensors": {
      "Barometer": {
          "SensorType": 1,
          "Enabled": true,
          "PressureFactorSigma": 0.001825,
          "PressureFactorTau": 3600,
          "UncorrelatedNoiseSigma": 2.7,
          "UpdateLatency": 0,
          "UpdateFrequency": 50,
          "StartupDelay": 0
      },
      "Imu": {
          "SensorType": 2,
          "Enabled": true,
          "AngularRandomWalk": 0.3,
          "GyroBiasStabilityTau": 500,
          "GyroBiasStability": 4.6,
          "VelocityRandomWalk": 0.24,
          "AccelBiasStabilityTau": 800,
          "AccelBiasStability": 36,
          "AccelTurnOnBiasX": 0, // m/s^2
          "AccelTurnOnBiasY": 0, // m/s^2
          "AccelTurnOnBiasZ": 0, // m/s^2
          "GyroTurnOnBiasX": 0, // deg/s
          "GyroTurnOnBiasY": 0, // deg/s
          "GyroTurnOnBiasZ": 0 // deg/s
      },
      "Gps": {
          "SensorType": 3,
          "Enabled": true,
          "EphTimeConstant": 0.9,
          "EpvTimeConstant": 0.9,
          "EphInitial": 25,
          "EpvInitial": 25,
          "EphFinal": 0.1,
          "EpvFinal": 0.1,
          "EphMin3d": 3,
          "EphMin2d": 4,
          "SigmaLong": 0.00003, // longitude random noise standard deviation (deg)
          "SigmaLat": 0.00003, // latitude random noise standard deviation (deg)
          "SigmaAlt": 1.0, // altitude random noise standard deviation (m)
          "SigmaVelX": 0.1, // gps velocity random noise standard deviation (m/s)
          "SigmaVelY": 0.1, // gps velocity random noise standard deviation (m/s)
          "SigmaVelZ": 0.1, // gps velocity random noise standard deviation (m/s)
          "UpdateLatency": 0,
          "UpdateFrequency": 10,
          "StartupDelay": 0
      },
      "Magnetometer": {
          "SensorType": 4,
          "Enabled": true,
          "NoiseSigma": 0.005,
          "ScaleFactor": 1,
          "NoiseBias": 0,
          "UpdateLatency": 0,
          "UpdateFrequency": 50,
          "StartupDelay": 0
      }
  },
  "EkfSetting": {
      "Enabled": true,
      "GpsFusion": true,
      "BaroFusion": true,
      "MagnetoFusion": true,
      "Imu": { // imu random error standard deviations
          "AccelErrorStdDevX": 0.1, // m/s^2
          "AccelErrorStdDevY": 0.1, // m/s^2
          "AccelErrorStdDevZ": 0.1, // m/s^2
          "GyroErrorStdDevX": 0.2, // deg/s
          "GyroErrorStdDevY": 0.2, // deg/s
          "GyroErrorStdDevZ": 0.2 // deg/s
      },
      "Gps": { // gps random error standard deviations
          "PositionErrorStdDevX": 4.0, // m
          "PositionErrorStdDevY": 4.0, // m
          "PositionErrorStdDevZ": 4.0, // m
          "VelocityErrorStdDevX": 0.2, // m/s
          "VelocityErrorStdDevY": 0.2, // m/s
          "VelocityErrorStdDevZ": 0.2 // m/s
      },
      "Barometer": { // barometer altitude random error standard deviation
          "PositionErrorStdDevZ": 2.0 // m
      },
      "Magnetometer": { // magnetometer magnetic flux random error standard deviations
          "MagFluxErrorStdDevX": 0.01, // Gauss
          "MagFluxErrorStdDevY": 0.01, // Gauss
          "MagFluxErrorStdDevZ": 0.01 // Gauss
      },
      "PseudoMeasurement": { // R matrix for Quaternion normalization
          "QuaternionNormR": 0.000001
      },
      "InitialStatesStdErr": { // standard deviations of initial states errors
          "PositionX": 6.0, // m
          "PositionY": 6.0, // m
          "PositionZ": 6.0, // m
          "LinearVelocityX": 1.0, // m/s
          "LinearVelocityY": 1.0, // m/s
          "LinearVelocityZ": 1.0, // m/s
          "QuaternionW": 0.05,
          "QuaternionX": 0.05,
          "QuaternionY": 0.05,
          "QuaternionZ": 0.05,
          "AccelBiasX": 0.1, // m/s^2
          "AccelBiasY": 0.1, // m/s^2
          "AccelBiasZ": 0.1, // m/s^2
          "GyroBiasX": 1, // deg/s
          "GyroBiasY": 1, // deg/s
          "GyroBiasZ": 1, // deg/s
          "BaroBias": 1 // m
      },
      "InitialStatesErr": { // initial states errors
          "PositionX": 0.0, // m
          "PositionY": 0.0, // m
          "PositionZ": 0.0, // m
          "LinearVelocityX": 0.0, // m/s
          "LinearVelocityY": 0.0, // m/s
          "LinearVelocityZ": 0.0, // m/s
          "AttitideRoll": 0.0, // deg
          "AttitidePitch": 0.0, // deg
          "AttitideYaw": 0.0, // deg
          "AccelBiasX": 0, // m/s^2
          "AccelBiasY": 0, // m/s^2
          "AccelBiasZ": 0, // m/s^2
          "GyroBiasX": 0, // deg/s
          "GyroBiasY": 0, // deg/s
          "GyroBiasZ": 0, // deg/s
          "BaroBias": 0.0 // m
      }
  }
  }

• (Optional but recommended): install the local airsim client. There is an additional resetVehicleApi() that resets vehicle and thus EKF if required.

Demo

• Set the following settings as desired:

  "EkfSetting": {
    "Enabled": true, // true to use ekf estimated states in other components, else use ground truth
    "GpsFusion": true, // true to fuse gps measurements
    "BaroFusion": true, // true to fuse barometer measurement
    "MagnetoFusion": true // true to fuse magnetometer measurements
  }

• Run hello_drone_ekf.py as the client code.
• Run plot.py to plot the EKF states and the measurements.

Known Issues

• While I reset the Sim-World using client.reset() here, I hit the failResetUpdateOrdering("Multiple reset() calls detected without call to update()"); here, of one of the sensors due to two consecutive resets without update. There is, however, a hint that this check could be redundant.

• I get the following error as a result of the Github actions for Windows build:

  Build FAILED.
  
     "D:\a\AirSim\AirSim\AirSim.sln" (default target) (1) ->
     "D:\a\AirSim\AirSim\AirLibUnitTests\AirLibUnitTests.vcxproj" (default target) (11) ->
     (ClCompile target) -> 
       D:\a\AirSim\AirSim\AirLibUnitTests\main.cpp : fatal error C1128: number of sections exceeded object file format limit: compile with /bigobj [D:\a\AirSim\AirSim\AirLibUnitTests\AirLibUnitTests.vcxproj]
  
  0 Warning(s)
  1 Error(s)

There is already a solution mentioned, but I am not sure about this.

[jonyMarino]

Thank you very much @subedisuman, for the detailed update. You are doing an excellent job! Keep it up!

[subedisuman]

Hi @jonyMarino. Thank you for your kind remarks. Regarding the recent announcement to close the support for this repository, I wonder how it affects this potential pull request? I would love to get your feedback regarding this issue and the new strategy. The current implementation can make use of the AirSim imu, gps, barometer, and magnetometer sensor signals to compute and use estimated states based on configurable EKF parameters. Ongoing work-in-progress is to test the implementation. If possible, I would also love to get your feedbacks regarding the functionality, the implementation, the procedures to test it, and the next steps regarding the potential pull request. Thank you.