Estimation Project

Welcome to the estimation project. In this project, you will be developing the estimation portion of the controller used in the CPP simulator. By the end of the project, your simulated quad will be flying with your estimator and your custom controller (from the previous 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 Tasks - the tasks you will need to complete for the project
• Tips and Tricks - some additional tips and tricks you may find useful along the way
• Submission - overview of the requirements for your project submission

Setup

This project will continue to use the C++ development environment you set up in the Controls C++ project.

1. Clone the repository

git clone https://github.com/udacity/FCND-Estimation-CPP.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. For this project, you will be working with simulations 06 through 11 and you may find it insightful to take a look at the configuration for 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 Tasks

Once again, you will be building up your estimator in pieces. At each step, there will be a set of success criteria that will be displayed both in the plots and in the terminal output to help you along the way.

Project outline:

• Step 1: Sensor Noise
• Step 2: Attitude Estimation
• Step 3: Prediction Step
• Step 4: Magnetometer Update
• Step 5: Closed Loop + GPS Update
• Step 6: Adding Your Controller

Step 1: Sensor Noise

1. Choose scenario 06_NoisySensors. In this simulation, the interest is to record some sensor data on a static quad, so you will not see the quad move. Run this scenario, the graphs will be recorded to the following csv files with headers: config/log/Graph1.txt (GPS X data) and config/log/Graph2.txt (Accelerometer X data).

2. Process the logged files to figure out the standard deviation of the the GPS X signal and the IMU Accelerometer X signal.

3. Plug in my result into the top of config/6_Sensornoise.txt. Specially, set the values for MeasuredStdDev_GPSPosXY and MeasuredStdDev_AccelXY to be the values I have calculated.

4. The dashed lines in the simulation will eventually turn green, indicating we are capturing approx 68% of the respective measurements (which is what we expect within +/- 1 sigma bound for a Gaussian noise model)

GPS X Std: 0.7210596171058772
Accelerometer X Std: 0.5084086769833626

Simulation #11 (../config/06_SensorNoise.txt)
PASS: ABS(Quad.GPS.X-Quad.Pos.X) was less than MeasuredStdDev_GPSPosXY for 67% of the time
PASS: ABS(Quad.IMU.AX-0.000000) was less than MeasuredStdDev_AccelXY for 69% of the time

Step 2: Attitude Estimation

Now let's look at the first step to our state estimation: including information from our IMU. In this step, I will be improving the complementary filter-type attitude filter with a better rate gyro attitude integration scheme.

1. Run scenario 07_AttitudeEstimation. For this simulation, the only sensor used is the IMU and noise levels are set to 0 (see config/07_AttitudeEstimation.txt for all the settings for this simulation). There are two plots visible in this simulation.

   • The top graph is showing errors in each of the estimated Euler angles.
   • The bottom shows the true Euler angles and the estimates. Observe that there is quite a bit of error in attitude estimation.

2. In QuadEstimatorEKF.cpp to reduce the errors in the estimated attitude (Euler Angles), implement a better rate gyro attitude integration scheme. The code is in QuadEstimatorEKF.cpp line 95 to 100

Simulation #5 (../config/07_AttitudeEstimation.txt)
PASS: ABS(Quad.Est.E.MaxEuler) was less than 0.100000 for at least 3.000000 seconds

Step 3: Prediction Step

In this next step I implemented the prediction step of my filter.

1. Run scenario 08_PredictState. This scenario is configured to use a perfect IMU (only an IMU). Due to the sensitivity of double-integration to attitude errors, we've made the accelerometer update very insignificant (QuadEstimatorEKF.attitudeTau = 100). The plots on this simulation show element of your estimated state and that of the true state. At the moment you should see that your estimated state does not follow the true state.

2. In QuadEstimatorEKF.cpp, implement the state prediction step in the PredictState() functon. Predict the current state forward by time dt using current accelerations, rotation matrix Rbg and body rates as input:

• The Rotation matrix:

I used attitude.Rotate_BtoI() to rotate the acceleration from body frame to global frame. The code is in QuadEstimatorEKF.cpp line 166 to 174

• Then take the Jacobian:

3. Now let's introduce a realistic IMU, one with noise. Run scenario 09_PredictionCov. You will see a small fleet of quadcopter all using your prediction code to integrate forward. You will see two plots:

   • The top graph shows 10 (prediction-only) position X estimates
   • The bottom graph shows 10 (prediction-only) velocity estimates You will notice however that the estimated covariance (white bounds) currently do not capture the growing errors.

4. In QuadEstimatorEKF.cpp, calculate the partial derivative of the body-to-global rotation matrix in the function GetRbgPrime(). Once you have that function implement, implement the rest of the prediction step (predict the state covariance forward) in Predict(). The code is in QuadEstimatorEKF.cpp line 200 to 205

5. Run your covariance prediction and tune the QPosXYStd and the QVelXYStd process parameters in QuadEstimatorEKF.txt to try to capture the magnitude of the error you see. Note that as error grows our simplified model will not capture the real error dynamics (for example, specifically, coming from attitude errors), therefore try to make it look reasonable only for a relatively short prediction period (the scenario is set for one second).

Step 4: Magnetometer Update

Up until now we've only used the accelerometer and gyro for our state estimation. In this step, you will be adding the information from the magnetometer to improve your filter's performance in estimating the vehicle's heading.

1. Run scenario 10_MagUpdate. This scenario uses a realistic IMU, but the magnetometer update has not been implemented yet. As a result, you will notice that the estimate yaw is drifting away from the real value (and the estimated standard deviation is also increasing). Note that in this case the plot is showing you the estimated yaw error (quad.est.e.yaw), which is drifting away from zero as the simulation runs. You should also see the estimated standard deviation of that state (white boundary) is also increasing.

• By reading the magnetometer reporting YAW in the global frame:

• Since this is linear, the derivative is a matrix of zero and one:

2. Tune the parameter QYawStd (QuadEstimatorEKF.txt) for the QuadEstimatorEKF so that it approximately captures the magnitude of the drift, as demonstrated here:

3. Implemented magnetometer update in the function UpdateFromMag(). The resulting plot is as follow:

The code is in QuadEstimatorEKF.cpp line 312 to 319

Simulation #4 (../config/10_MagUpdate.txt)
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 72% of the time

Step 5: Closed Loop + GPS Update

Run scenario 11_GPSUpdate and assume we get postion and velocity from GPS and using heading of the GPS but not the drone's orientation, only the direction of travel. we removing it from observation.

• Then the partial derivative is the identity matrix:

The code is in QuadEstimatorEKF.cpp line 281 to 291 and successfully completed the entire simulation cycle with estimated position error of < 1m.

Simulation #3 (../config/11_GPSUpdate.txt)
PASS: ABS(Quad.Est.E.Pos) was less than 1.000000 for at least 20.000000 seconds

Step 6: Adding Your Controller

Up to this point, we have been working with a controller that has been relaxed to work with an estimated state instead of a real state. So now, you will see how well your controller performs and de-tune your controller accordingly.

1. Replace QuadController.cpp with the controller you wrote in the last project.

2. Replace QuadControlParams.txt with the control parameters you came up with in the last project.

3. Run scenario 11_GPSUpdate. If your controller crashes immediately do not panic. Flying from an estimated state (even with ideal sensors) is very different from flying with ideal pose. You may need to de-tune your controller. Decrease the position and velocity gains (we have seen about 30% detuning being effective) to stabilize it. Your goal is to once again complete the entire simulation cycle with an estimated position error of < 1m.

After tune GPSPosZStd to 300, maxTiltAngle = 1.0, QYawStd = 0.15, position and velocity gains pass the step 6 here is my result:

Simulation #17 (../config/11_GPSUpdate.txt)
PASS: ABS(Quad.Est.E.Pos) was less than 1.000000 for at least 20.000000 seconds

Tips and Tricks

• When it comes to transposing matrices, .transposeInPlace() is the function you want to use to transpose a matrix

• The Estimation for Quadrotors document contains a helpful mathematical breakdown of the core elements on your estimator

Submission

For this project, you will need to submit:

• a completed estimator that meets the performance criteria for each of the steps by submitting:

  • QuadEstimatorEKF.cpp
  • config/QuadEstimatorEKF.txt

• a re-tuned controller that, in conjunction with your tuned estimator, is capable of meeting the criteria laid out in Step 6 by submitting:

  • QuadController.cpp
  • config/QuadControlParams.txt

• a write up addressing all the points of the rubric

Authors

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