Estimation Project

This README is broken down into the following sections:

• Setup - the environment and code setup required to get started and a brief overview of the project structure.
• The Scenarios - the scenarios that test the performance of the controller and estimators.

Setup

1. Clone the repository

git clone https://github.com/l0g1c-80m8/3d-drone-estimation.git

2. Import the code into your IDE like done in the Controls C++ project

3. You should now be able to compile and run the estimation simulator just as you did in the controls project

Project Structure

For this project, you will be interacting with a few more files than before.

• The EKF is already partially implemented for you in QuadEstimatorEKF.cpp

• Parameters for tuning the EKF are in the parameter file QuadEstimatorEKF.txt

• When you turn on various sensors (the scenarios configure them, e.g. Quad.Sensors += SimIMU, SimMag, SimGPS), additional sensor plots will become available to see what the simulated sensors measure.

• The EKF implementation exposes both the estimated state and a number of additional variables. In particular:

  • Quad.Est.E.X is the error in estimated X position from true value. More generally, the variables in <vehicle>.Est.E.* are relative errors, though some are combined errors (e.g. MaxEuler).

  • Quad.Est.S.X is the estimated standard deviation of the X state (that is, the square root of the appropriate diagonal variable in the covariance matrix). More generally, the variables in <vehicle>.Est.S.* are standard deviations calculated from the estimator state covariance matrix.

  • Quad.Est.D contains miscellaneous additional debug variables useful in diagnosing the filter. You may or might not find these useful but they were helpful to us in verifying the filter and may give you some ideas if you hit a block.

config Directory

In the config directory, in addition to finding the configuration files for your controller and your estimator, you will also see configuration files for each of the simulations. In this project, we address scenarios 06 through 11 in the simulation.

As an example, if we look through the configuration file for scenario 07, we see the following parameters controlling the sensor:

# Sensors
Quad.Sensors = SimIMU
# use a perfect IMU
SimIMU.AccelStd = 0,0,0
SimIMU.GyroStd = 0,0,0

This configuration tells us that the simulator is only using an IMU and the sensor data will have no noise. You will notice that for each simulator these parameters will change slightly as additional sensors are being used and the noise behavior of the sensors change.

The Scenarios

Outline:

• Scenario 6: Sensor Noise
• Scenario 7: Attitude Estimation
• Scenarios 8 and 9: Prediction Step
• Scenario 10: Magnetometer Update
• Scenario 11: Closed Loop + GPS Update
• Scenario 11: Adding Your Controller

Scenario 6: Sensor Noise

The logs generated by running scenario 6 are analyzed to calculate the standard deviation of the GPS and IMU sensor measurements. These values are then used to update the sensor noise parameters:

MeasuredStdDev_GPSPosXY = 0.714
MeasuredStdDev_AccelXY = .512

The measurements observed consequently are within acceptable range:

PASS: ABS(Quad.GPS.X-Quad.Pos.X) was less than MeasuredStdDev_GPSPosXY for 68% of the time
PASS: ABS(Quad.IMU.AX-0.000000) was less than MeasuredStdDev_AccelXY for 70% of the time

Scenario 7: Attitude Estimation

In this scenario, the UpdateFromIMU() function is refactored to implement a better integration scheme that uses current attitude estimates:

• First, the attitude estimates are converted to quaternion form from the Euler angles.
• Then, the measurements in gyro are integrated for time dtIMU using the IntegrateBodyRate() function.
• Finally, the Euler angles are recovered back from the quaternion form.

The test passes successfully after implementing the new integration scheme:

PASS: ABS(Quad.Est.E.MaxEuler) was less than 0.100000 for at least 3.000000 seconds

Scenarios 8 and 9: Prediction Step

Here the prediction step of the filter is checked.

There are four steps to take here:

1. PredictState(): Here the state prediction step is implemented. Here the accelerations, current state and vehicle attitude angles are used to predict the forward state of the vehicle. After completing this step, it is observed that the estimator state tracks the actual state, with only reasonably slow drift:
2. GetRbgPrime(): Here the partial derivative of the Rbg rotation matrix with respect to yaw is calculated, called the RbgPrime matrix to be used later in the next step. This function implements the matrix calculation as shown in equation 52.
3. Predict(): Here the calculation of current covariance forwarded by time dt is done using the accelerations and body rates as input according to equation 51 using the RbgPrime matrix calculated in step 2 above.
4. Finally, the parameters QPosXYStd and QVelXYStd are tuned to get the covariance (white line) grow like the data. It should look as below:

Success criteria: This step doesn't have any specific measurable criteria being checked.

Scenario 10: Magnetometer Update

Here the magnetometer update is included. This requires 2 steps:

1. The parameter QYawStd is tuned to appropriately capture the magnitude of the drift:
2. Next, the UpdateFromMag is completed to include the magnetometer update following the equations described in section 7.3.2.

After the parameter (QYawStd) tuning test passes successfully:

PASS: ABS(Quad.Est.E.Yaw) was less than 0.120000 for at least 10.000000 seconds
PASS: ABS(Quad.Est.E.Yaw-0.000000) was less than Quad.Est.S.Yaw for 59% of the time

Scenario 11: Closed Loop + GPS Update

This is the final scenario to test. The steps taken here are:

1. Quad.UseIdealEstimator is set to zero to use the estimator implemented thus far.
2. SimIMU.AccelStd and SimIMU.GyroStd are commented (only for testing) to have a realistic model of an IMU to test the estimator with.
3. The process noise model parameters are tuned to capture the error observed with the estimated uncertainty of the filter.
4. UpdateFromGPS() is completed to include GPS updates.

After all of these tasks, the scenario passes as seen below:

with the following message:

ABS(Quad.Est.E.Pos) was less than 1.000000 for at least 20.000000 seconds

Passing all scenarios stated above indicates a working estimator.

Scenario 12: Adding Your Controller

Instead of using a controller relaxed to work with estimated state, the controller from the controls repository is used now in the same scenario. The parameters in QuadControlParams.txt are tuned here to work with the estimator developed thus far. The same scenario 11 is run where the following result is observed:

along with the PASS message for the test:

PASS: ABS(Quad.Est.E.Pos) was less than 1.000000 for at least 20.000000 seconds

Authors

Thanks to Fotokite for the initial development of the project code and simulator.