4th Project - Building an Estimator
Detailed Project Explantion Page
This is the last project of the Flying Car Nano Degree (FCND) - Term1. In this project, by using Extended Kalman Filter (EKF) estimation method, the estimation portion of the controller which is used in the CPP simulator is developed.By the end of the project, my simulated quad flies with my estimator and my custom controller. This project consists of 6 steps [1] .
- 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
Each step is given below detailed.
In this step , collecting some simulated noisy sensor data (GPS and IMU measurements) and estimate the standard deviation of those sensor.
Standard Deviation Calculation is obtaion by using std_cal.py code. To obation standard deviation , numpy.std[2] is used.
GPS X Standard Deviation (MeasuredStdDev_GPSPosXY): 0.6797007868796459
ACC X Standard Deviation (MeasuredStdDev_AccelXY): 0.475746035407147
Success Criteria:
* Standard deviations should accurately capture the value of approximately 68% of the respective measurements.
Result:
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 67% of the time
In this setup , the complementary filter-type attitude filter is improved by integrating body rate p,q,r which obtained from rate gyro into the estimated pitch and roll angle.
By using this equation[3], an instantaneous change in the Euler angles (world frame) is obtained from turn rate in the body frame.
After obtained Euler Rate, integrating into the estimated pitch and roll angle.
// Predict
float predictedPitch = pitchEst + dtIMU * euler_dot.y;
float predictedRoll = rollEst + dtIMU * euler_dot.x;
ekfState(6) = ekfState(6) + dtIMU * euler_dot.z; // yawThe implementation of this step is at QuadEstimatorEKF.cpp from line 74 to line 137.
Success Criteria:
* Your attitude estimator needs to get within 0.1 rad for each of the Euler angles for at least 3 seconds.
Result:
PASS: ABS(Quad.Est.E.MaxEuler) was less than 0.100000 for at least 3.000000 seconds
This step consists of two scenarios;
The first scenario , implementing the state prediction based on the acceleration measurement by using Dead Reckoning method [4]
The implementation of this step is at QuadEstimatorEKF.cpp from line 162 to line 203.
//From Lesson 17 - Dead Reckoning Exercise
predictedState(0) = curState(0) + curState(3) * dt; // x coordianate x= x + x_dot * dt
predictedState(1) = curState(1) + curState(4) * dt; // y coordianate y= y + y_dot * dt
predictedState(2) = curState(2) + curState(5) * dt; // z coordianate z= z + z_dot * dt
//Convert the true acceleration from body frame to the inertial frame
V3F acc_inertial = attitude.Rotate_BtoI(accel);
predictedState(3) = curState(3) + acc_inertial.x * dt; // change in velocity along the x is a_x * dt
predictedState(4) = curState(4) + acc_inertial.y * dt; // change in velocity along the y is a_y * dt
predictedState(5) = curState(5) + acc_inertial.z * dt - CONST_GRAVITY * dt; // change in velocity along the z is a_z * dt by removing the gravity component
The second scenario,
The first step , calculating the partial derivative of the body-to-global rotation matrix in the function GetRbgPrime() by using equations Section 7.2 which is given in Estimation for Quadrotors paper [5]
The implementation of this step is at QuadEstimatorEKF.cpp from line 205 to line 245.
// From "Estimation for Quadrotors" paper ( Eq. 52 )
float theta = pitch;
float phi = roll ;
float psi = yaw ;
RbgPrime(0,0) = (- ( cos(theta) * sin(psi) ) );
RbgPrime(0,1) = (- ( sin(phi) * sin(theta) * sin(psi) ) - ( cos(theta) * cos(psi) ) );
RbgPrime(0,2) = (- ( cos(phi) * sin(theta) * sin(psi) ) + ( sin(phi) * cos(psi) ) );
RbgPrime(1,0) = ( cos(theta) * cos(psi) ) ;
RbgPrime(1,1) = ( sin(phi) * sin(theta) * cos(psi) ) - ( cos(phi) * sin(psi) );
RbgPrime(1,2) = ( cos(phi) * sin(theta) * cos(psi) ) + ( sin(phi) * sin(psi) );
RbgPrime(2,0) = 0;
RbgPrime(2,1) = 0;
RbgPrime(2,2) = 0;The second step, obtaining Jacobian Matrix by using GetRbgPrime() and after iplementing prediction step (predict the state covariance forward) to update the covariance matrix cov according to the EKF equation. For Jacobian Matrix equations in Section 7.2 and prediction equations in Section 3 which are given in Estimation for Quadrotors paper [5]
The implementation of this step is at QuadEstimatorEKF.cpp from line 247 to line 300.
// From "Estimation for Quadrotors" paper ( Eq. 51 )
gPrime(0,3) = dt;
gPrime(1,4) = dt;
gPrime(2,5) = dt;
gPrime(3, 6) = (RbgPrime(0) * accel).sum() * dt;
gPrime(4, 6) = (RbgPrime(1) * accel).sum() * dt;
gPrime(5, 6) = (RbgPrime(2) * accel).sum() * dt;
// From "Estimation for Quadrotors" paper ( Section 3 )
ekfCov = gPrime * ekfCov * gPrime.transpose() + Q;
Success Criteria:
* This step doesn't have any specific measurable criteria being checked.
In this step, the information from the magnetometer is added to improve filter's performance in estimating the vehicle's heading.
[5]
The implementation of this step is at QuadEstimatorEKF.cpp from line 340 to line 371.
hPrime(0, 6) = 1; // hPrime= [ 0 0 0 0 0 1]
zFromX(0) = ekfState(6);
//normalize the difference between your measured and estimated yaw
float diff = magYaw - zFromX(0);
if ( diff > F_PI ) {
zFromX(0) += 2.f*F_PI;
} else if ( diff < -F_PI ) {
zFromX(0) -= 2.f*F_PI;
}
Performance Evaluation:
* Your goal is to both have an estimated standard deviation that accurately captures the error and maintain an error of less than 0.1rad in heading for at least 10 seconds of the simulation.
Result:
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 57% of the time
In this step, GPS update is implemented into estimator. by the help of the equations Section 7.3.1 from Estimation for Quadrotors paper [5] , implementation is achieved.
The implementation of this step is at QuadEstimatorEKF.cpp from line 302 to line 338.
// From "Estimation for Quadrotors" paper ( Eq. 53 & Eq. 54 )
zFromX(0) = ekfState(0);
zFromX(1) = ekfState(1);
zFromX(2) = ekfState(2);
zFromX(3) = ekfState(3);
zFromX(4) = ekfState(4);
zFromX(5) = ekfState(5);
// From "Estimation for Quadrotors" paper ( Eq. 55 )
hPrime(0, 0) = 1;
hPrime(1, 1) = 1;
hPrime(2, 2) = 1;
hPrime(3, 3) = 1;
hPrime(4, 4) = 1;
hPrime(5, 5) = 1;
Success Criteria:
* Your objective is to complete the entire simulation cycle with estimated position error of < 1m.
Result:
PASS: ABS(Quad.Est.E.Pos) was less than 1.000000 for at least 20.000000 seconds
In this step, my controller which was designed in Project 3 (FCND-Controls ) performs and de-tune my controller accordingly.
Success Criteria:
* Your objective is to complete the entire simulation cycle with estimated position error of < 1m.
Result:
PASS: ABS(Quad.Est.E.Pos) was less than 1.000000 for at least 20.000000 seconds
References
- [1] https://github.com/udacity/FCND-Estimation-CPP
- [2] https://docs.scipy.org/doc/numpy-1.14.0/reference/generated/numpy.std.html
- [3] FCND Lesson 14 - 3D Drone Exercise- Part3
- [4] FCND Lesson 17 - Dead Reckoning 3D Exercise
- [5] S. Tellex, A. Brown and S. Lupashin, 2018, "Estimation for Quadrotors"