Mechatronics System Design

(MTS-315)

Assignment Report: Extended Kalman Filter

Category NS NS NS ASC
Name Talha Zubair Taaha Jawad Noman Saad
Khan Bashir Salahuddin
Reg No. 199253
Degree 38A
Date 12-06-2019
 1. Introduction
In this assignment, the task was to estimate values for the roll angle and
pitch angle of a quadrotor UAV, given the time steps, the gyroscope outputs (the
angular velocities about the x,y, and z axes respectively), the accelerometer outputs
(accelerations in the x,y, and z axes), and the true values of the roll and pitch
angles. In this assignment, this data was used to estimate the roll and pitch angles,
using an Extended Kalman Filter, and the estimated and true values were plotted to
visualize the accuracy of the Kalman estimates.
2. Procedure
The following steps were followed in determining the Kalman estimates:
1. The State Space variables (in this case: the roll angle (phi), and the pitch
angle(theta)) were defined as a variable x. This vector was given an initial
value of 0.
2. The Eular angle rate variables for the gyroscope outputs (phi dot and theta
dot) were declared and initialized as zero.
3. The required matrices, i.e. Q , R ,P_initial , were initialized using values
given in the data set.
4. The required data was extracted from the given table and assigned
appropriate vector variables.
5. Then, the main part of the filter was written inside a for loop, since the EKF
is an iterative process.
6. The operation of the EKF can be split into two parts. The Prediction step,
and the Correction step.
7. The Prediction Step:
a. Phi dot and Theta dot were calculated using the given transfer
function, which converts the gyroscope outputs (p, q ,r) into Euler
angle rates. This was computed using the initial values for the state
variables.
b. Then, the values of the state variables (phi and theta) were predicted
using the following equations:
𝜑𝑘 − 𝜑𝑘−1
𝜑̇ =
∆𝑡
The equation above was re arranged for 𝜑𝑘 , i.e. the value of the roll
angle in the next time step. 𝜑𝑘 was then determined as all the other
values were known (using the initial values of 𝜑𝑘−1 . A similar
procedure was repeated for 𝜃𝑘
 c. Then, the state transformation matrix, F, was determined by taking the
Jacobian of the transfer function of the gyroscope. This was adjusted
with the time step, and the identity matrix.
d. The error covariance matrix was then calculated
8.
a. The measurement model used for the accelerometer was:
ℎ(𝑥) = [tan(𝜑) , 𝑔𝑠𝑖𝑛(𝜃)]𝑇
b. The C matrix was then calculated, which is the Jacobian of h(x)
c. The Kalman gain was then calculated. The measurement of tan(𝜑),
and 𝑔𝑠𝑖𝑛(𝜃) was then determined from the accelerometer outputs.
This was the compared with h(x), and the Kalman gain to produce a
corrected value of x. Thus, the corrected state variables were
calculated, followed by the corrected error covariance matrix.
d. These values of the state variables were stored in an array.
e. Once the loop terminated, the true values, and estimated values were
plotted on a graph.
3. Results
Fig. 1 shows the true and estimated values of the pitch angle, against time.
Fig. 2 shows the true and estimated values of the roll angle, against time. In both
figures, the blue line represents the true value of the corresponding angle, while the
red line represents the value estimated by the Extended Kalman Filter.
Figure 1 Pitch angle vs Time

Figure 2 Roll angle vs Time

 4. Analysis
The error between the EKF estimate, and true value of the roll angle, is
relatively small, compared to the pitch angle. The EKF estimate of the roll angle
generally follows the trend of the actual roll angle quite well, though the estimate
seems to be generally offset above the true roll angle. Adjusting the values of the Q
and R matrices has pronounced effects on the final EKF estimate.
For the Pitch angle, the difference between the EKF estimate and the true
value of the pitch angle is quite profound, during the beginning, with the error
reducing to an acceptable level at around sixty seconds.
Many difficulties were encountered in this assignment. Firstly, the
determination of the system model to be used caused some difficulty, since the
given kinematic model was the dynamic model for the gyroscope. This was
overcome by assuming the gyroscope model to be the system model, and the
accelerometer model to be the measurement model. In addition, a lot of difficulty
was encountered in the EKF estimate, since it was initially divergent. This was
resolved by including the following correction to the F matrix:
F = I + (F * dt);
Finally, the determination of the correct measurement model caused issues. This
was resolved by using the internet to determine how the output of the
accelerometer could be used to determine the values of the roll and pitch angles.
This knowledge was then used to develop the h(x) function, and the measurement
model of the accelerometer.

5. Conclusion

In conclusion, the Extended Kalman Filter is a very powerful tool to

estimate the values of state variables if there is non-linearity in the system, as
shown by the plots in the results section of this report. This assignment helped us
understand how the filter is implemented, and what difficulties arise during its
implementation. See Appendix I for the MATLAB code.
Appendix I
%Start of Script
%---------------------------Initialization of variables-------------------

x = [0;0]; %state variable vector

phi = x(1,1);

theta = x(2,1);

phi_dot = 0;

theta_dot = 0;

Estimate = zeros(2,44676); %Stores all estimates of the state variable

dt = 0.002; %Timestep

%---------------------------Initialization of Matrices-------------------
P = [1e-9,0;0,1e-9]; %Values from given dataset
Q = [1e-15,0;0,1e-15];
R = [1e-8,0;0,1e-6];
I = [1,0;0,1]; %Identity matrix

p = zeros(1,44676);
q = zeros(1,44676);
r = zeros(1,44676);
ax = zeros(1,44676);
ay = zeros(1,44676);
az = zeros(1,44676);
true_phi = zeros(1,44676);
true_theta = zeros(1,44676);

%-------------------------Extracting needed data from given dataset------

for i = 1:44676
p(i) = UAV_DATA(2,i);
q(i) = UAV_DATA(3,i);
r(i) = UAV_DATA(4,i);
ax(i) = UAV_DATA(5,i);
ay(i) = UAV_DATA(6,i);
az(i) = UAV_DATA(7,i);
true_phi(i) = UAV_DATA(8,i);
true_theta(i) = UAV_DATA(9,i);

end
%---------------------------The EKF-------------------
for j = 1:44676

%Prediction step

phi = x(1,1);
theta = x(2,1);
phi_dot = p(j) + (q(j) * sin(phi) * tan(theta)) + (r(j) * cos(phi) *
tan(theta)); %from gyro
theta_dot = (q(j) * cos(phi)) - (r(j) * sin(phi));%from gyro

%Predicting state variables

x(1,1) = (phi_dot * dt) + x(1,1);

x(2,1) = (theta_dot * dt) + x(2,1);
phi = x(1,1);
theta = x(2,1);

%Determining the State transformation matrix as the Jacobian of the

%given function

F = [((q(j) * cos(phi) * tan(theta)) - (r(j) * sin(phi) * tan(theta))),

((q(j) * sin(phi) * ((sec(theta))^2)) + ((r(j) * cos(phi) *
((sec(theta))^2))));(-1 * (q(j) * sin(phi))) - (r(j) * cos(phi)),0];

%Applying modification as shown in class

F = I + (F * dt);

%Predicting the Error Covariance

P1 = F * P * F' + Q;

%Calculating error covariance of the measurement model, as Jacobian of

%h(x)

C = [((sec(phi))^2),0;0,(9.81*cos(theta))];

%Determining the Kalman Gain

K = (P1 * C') / (R + C * P1 * C');

%Determining the measurement from accelerometer
y = [ay(j)/az(j);ax(j)] ;

h = [tan(phi);9.81*sin(theta)]; %transforming prediction

%Correction Step

x = x + K*(y-h); %Corrected value of x

P=P1 * (I-K*C) * (I-K*C)' + K * R * K';%Corrected Error Covariance
%Storing the state variables
Estimate(1,j) = x(1,1);
 Estimate(2,j) = x(2,1);
end
%Plotting the estimates and the true values of the angles.
Roll = figure();
Roll = plot(UAV_DATA(1,:),true_phi)
hold on
Roll = plot(UAV_DATA(1,:),Estimate(1,:),'red')
Pitch = figure();
Pitch = plot(UAV_DATA(1,:),true_theta)
hold on
Pitch = plot(UAV_DATA(1,:),Estimate(1,:),'red')