1) The document presents the implementation of a Generalized Multiplicative Extended Kalman Filter (GMEKF) for state estimation of a quadcopter using real sensor data. The GMEKF uses quaternion representations and accounts for nonlinearities and sensor noise. 2) Testing the GMEKF on recorded quadcopter flight data showed the filter did not converge, likely due to inconsistencies in the initial sensor and system noise matrices used. Further work is needed to properly implement the GMEKF equations and account for quaternion geometry. 3) Proper implementation of invariant extended Kalman filters like the GMEKF has been shown to provide good state estimation for various quadcopter trajectories by exploiting symmetries in the system dynamics.

1. 1
Generalized Multiplicative Extended Kalman Filter Implementation
using real quadrotor data
Hans Guentert, Matt Dechering, Gulnaaz Afzal
University of Cincinnati, 45226 Cincinnati, OH, USA
Nomenclature
ω -Instantaneous angular velocity vector, rad/s
ωb -Constant vector bias on gyroscopes, rad/s
ωm -Instantaneous angular velocity vector measured by the gyroscopes, rad/s
A -Constant gravity vector in North-East-Down coordinates, A = ge3, m/s2
a -Specific acceleration vector (all the non-gravitational forces divided by the body mass), m/s2
am -Specific acceleration vector measured by the accelerometers, m/s2
as -Positif constant scaling factor on accelerometers
B -Earth magnetic field in NED coordinates, T
ei -Unit vectors pointing respectively North, East, Down, i=1,2,3
hb -Constant scalar bias on altitude measurement provided by the barometric sensor, m
q -Unit quaternion representing the orientation of the body-fixed frame with respect to the Earth-fixed frame
V,X -Velocity and position vectors of the center of mass with respect to the Earth-fixed frame, m/s,m
Motivation
Uncertainty is a major obstacle for the smooth functioning of a quadcopter. Therefore, to
combat uncertainty in the form of wind disturbance, imprecision of actuators, or any other general
disturbance which cannot be modelled, we attempt to find a dynamic system control through the
system observer that can adjust itself to the changing environment. Since sensors sometimes offer
incomplete or imprecise data as well as issues due to the time delay between issuing motor commands
and receiving sensory feedback, system needs a way to predict the state. In the light of these limitations,
the approach to these challenges with the help of a powerful tool – Kalman Filtering. The quadrotor has
inherently nonlinear dynamics, so an extended Kalman filter system must be used. It provides the
needed model for making estimates of the current state of the motor system and issuing updated
commands.
Quaternions are widely used in the aerospace industry to represent rotations. They have
advantages over Euler angles such that they don’t have problems with Gimbal lock or any of singularities
associated with them. This means it is better to use a multiplicative error term because subtraction does

2. 2
not accurately depict the difference between quaternions. An EKF with this kind of multiplicative error
term is known as a Multiplicative EKF. In addition to position, velocity, and acceleration, the angle of the
quadcopter must be represented in the inertial reference frame. This could be represented with three
Euler angles. Quaternions are less intuitive, but more mathematically robust state representation. They
avoid the “Gimbal lock” problem – Two axes of a gyroscope becoming aligned resulting in an incorrect
rotation.
Generalized MEKF concepts were selected since quadcopters are low flying aircraft. Earth is
assumed to be flat; therefore, certain invariances in noise determination of the system could be
assumed, these intrinsic observations can be translated, rotated, or scaled.
Quadcopter Overview
A quadcopter is a typical UAV that flies through the air with four rotors. It, therefore, requires
reference frames to describe its state with relationship to the surrounding environment.
The quadcopter dynamics are derived by these two frames of its operation: Inertial frame and
Body frame. The inertial frame is defined by the ground, with gravity pointing in the negative z direction.
The body frame is defined by the orientation of the quadcopter, with the rotor axes pointing in the positive
z direction and the arms pointing in x and y directions. Since the sensors are tri-axial and fixed to the
quadcopter or referenced as the body frame, their data must be translated to the navigational frame or
inertial frame. The typical sensors used for quadcopter state observation are Inertial Measurement Unit
(IMU), Magnetometer, and Barometer. Quadcopter senses the given parameters in its own body frame,
which is separate from the inertial frame of reference.

3. 3
Quadcopter Body Frame and Inertial Frame Kinematics
The formalized kinematics of the body and inertial frames define the position and velocity of the
quadcopter in the inertial frame as x = (x, y, z) T and x˙ = (x˙, y˙, z˙) T, respectively. Similarly, roll, pitch,
and yaw angles are defined in the body frame as θ = (φ, θ, ψ) T, with corresponding angular velocities
being represented as ˙θ = (φ˙, ˙θ, ψ˙) T. However, note that the angular velocity vector (𝜔 = 𝜃̇) points
along the axis of rotation, while ˙θ denotes time derivative of yaw, pitch, and roll. To convert these
angular velocities into the angular velocity vector, we can use the following relation:
Ꙍ =
1 0 −𝑠 𝜃
0 𝑐∅ 𝑐 𝜃 𝑠∅
0 −𝑠∅ 𝑐 𝜃 𝑐∅
The body and inertial frame can be related by a rotation matrix R which goes from the body frame to the
inertial frame. This matrix is derived by using the ZYZ Euler angle conventions and successively “undoing”
the yaw, pitch, and roll motions.
R =
𝐶∅ 𝐶 𝜑 − 𝐶 𝜃 𝑆∅ 𝑆 𝜑 −𝐶 𝜑 𝑆∅ − 𝐶 𝜃 𝐶∅ 𝑆 𝜑 𝑆 𝜃 𝑆 𝜑
𝐶 𝜃 𝐶 𝜑 𝑆∅ + 𝐶∅ 𝑆 𝜑 𝐶∅ 𝐶 𝜃 𝐶 𝜑 − 𝑆 𝜑 𝑆∅ −𝐶 𝜑 𝑆 𝜃
𝑆∅ 𝑆 𝜃 𝐶∅ 𝑆 𝜃 𝐶 𝜃
For a given vector 𝑣⃗ in the body frame, the corresponding vector is given by R𝑣⃗ in the inertial frame.
Quaternions
In addition to position, velocity, and acceleration, the angle of the quadcopter must be
represented in the inertial reference frame. A unit quaternion, q ∈ 𝐻, represents the orientation in 3-D
space. It can be viewed as a scalar 𝑝0 ∈ 𝑅 and a vector 𝑝⃗ ∈ 𝑅3
.
The quaternion multiplication of p and q is denoted as,

4. 4
where, the identity element is 𝑒 =
1
0⃗⃗
To rotate to a new orientation, q must be multiplied by a different quaternion to represent the change
in orientation. The derivative of a quaternion for a moving system of coordinates is given by:
𝑑𝑞
𝑑𝑡
=
1
2
𝑞(𝑡)𝑤(𝑡)
Where, w(t) is the angular velocity vector [0, 𝑤 𝑥, 𝑤 𝑦, 𝑤𝑧]
Unlike Euler angles, quaternion error is not well represented by:
𝑞 𝑚𝑒𝑎𝑠 − 𝑞 𝑎𝑐𝑡
To rotate a vector from one frame to another, it can be multiplied by the quaternion rotation term:
𝑣 𝑤 = 𝑞 ∗ 𝑣 𝑏 ∗ 𝑞−1
Where 𝑣 𝑤 is the vector in the inertial frame, 𝑣 𝑏 is the vector in the body frame, and q is the
quaternion representing the rotation between the two frames.
Extended Kalman Filter
Quaternions are nonlinear, just like any other system using Euler angles; so for a Kalman filter,
the system must be linearized. The typical error term 𝑞 𝑧 − 𝑞( 𝑘| 𝑘 − 1) makes no sense for quaternions
when used with a Kalman gain. To overcome the error, a multiplicative error term is employed:
𝑞 𝑧
−1
∗ 𝑞( 𝑘| 𝑘 − 1)
Full system and IEKF general structure
Considering an equation on a matrix Lie Group G ⊂ 𝑅 𝑁∗𝑁
of the form:
𝒅
𝒅𝒕
𝑿𝒕 = 𝒇 𝒖𝒕(𝑿𝒕)
with 𝒇 𝒖(𝒂𝒃) = 𝒂𝒇 𝒖(𝒃) + 𝒇 𝒖(𝒂)𝒃 − 𝒂𝒇 𝒖(𝑰𝒅)𝒃 for all (u, a, b) € U*G*G.

5. 5
The main equations of Kalman Filter are defined to estimate the state in terms of the parameters
of Time and Measurement update
𝒙̇ 𝒏(𝒕) = 𝒇 𝒏(𝒙 𝒏(𝒕), 𝒕) + 𝒈 𝒏𝒙𝒑(𝒙 𝒏(𝒕), 𝒕)𝒘 𝒑(𝒕),
Where, 𝑥 𝑛(𝑡) ∈ 𝑅 𝑛
𝑖𝑠 𝑡ℎ𝑒 𝑠𝑡𝑎𝑡𝑒 𝑎𝑛𝑑 𝑊𝑝(𝑡) ∈ 𝑅 𝑛
𝑖𝑠 𝑡ℎ𝑒 𝑝𝑟𝑜𝑐𝑒𝑠𝑠 𝑛𝑜𝑖𝑠𝑒.
Time Update
Given 𝑋 𝑛,0 is the minimum covariance estimate of the state at a future time t in the absence of
measurements given by:
𝑥̂ 𝑛(𝑡) = 𝐸{𝑥 𝑛(𝑡)𝑥̂ 𝑛(𝑡0) = 𝑥 𝑛,0}.
This estimate satisfies the differential equation:
𝑥̂ 𝑛(𝑡) = 𝐸{𝑥 𝑛(𝑡)𝑥̂ 𝑛 , 𝑡)} which is approximated by:
𝒙̇ 𝒏(𝒕) ≈ 𝒇 𝒏(𝒙̂ 𝒏(𝒕), 𝒕)
Hence, in the absence of measurements, the state estimate is propagated using the above equation.
Measurement Update
Assume that a measurement is taken at time 𝑡 𝑘 that is related to the state ofthe EKF through the nonlinear
output equation,
𝑍 𝑚(𝑡 𝑘) = ℎ 𝑚(𝑥 𝑛(𝑡 𝑘)) + 𝑣 𝑚(𝑡 𝑘) ∈ 𝑅 𝑚
,
where, 𝑣 𝑚(𝑡 𝑘) ∈ 𝑅 𝑚
is the measurement noise assumed to be a discrete Gaussian White-Noise
Process.
Observers and State Estimation:
A Kalman filter with only accelerometer and gyroscope data provides a relatively weak estimate
of position, because of the double integration and nonlinearity of the system. An external observer with
an unchanging reference frame provides state feedback on the state estimate.

6. 6
Multiplicative Extended Kalman Filter
Using the quaternion error term 𝑞 𝑧
−1
∗ 𝑞( 𝑘| 𝑘 − 1) results in a multiplicative extended Kalman
filter. The filter seeks to make the quaternion (1,0,0,0), the quaternion multiplicative identity, rather
than 0, the additive identity seen in normal Kalman filters. All other error terms a linear (position,
velocity, acceleration, etc.) once they undergo transformation to the body’s reference frame. This does a
good job of preserving the geometric meaning of the quaternion, but the linearized matrices converge
only in level flight, because there are trajectory-dependent terms in the linearized A matrix.
Invariant Extended Kalman Filter
The invariant Kalman filter is an observer-based filter that employs an observer that is not
subject to the same change in dynamics as the system. The dynamics of the system are not changed
from inertial frame to the body frame. It looks at the system and is based around error terms that don’t
change with respect to rotation, transformation between body and inertial frame. A symmetry-
preserving observer uses the natural symmetries or invariances such as rotations, scaling, and
translation of the considered nonlinear model, the filter should take advantage of these properties. The
result is these properties leave the error system unchanged. This allows for larger domain of
convergence – trajectories with close to constant acceleration and/or rotation. The filter converges for
Level flight, and any permanent trajectory. It is associated with the following “noisy” system,

7. 7
𝒅
𝒅𝒕
𝑿𝒕 = 𝒇 𝒖𝒕(𝑿𝒕) + 𝑿𝒕 𝑾𝒕
Where, 𝑊𝑡 is a continuous white noise belonging to g whose covariance matrix is denoted by 𝑄𝑡, which
gives its output.
General Multiplicative Extended Kalman Filter:
The General Multiplicative Extended Kalman Filter expands on the Invariant EKF. It adds
additional sensor terms for barometer and position measurements. Implementation of the GMEKF was
attempted in MATLAB to check its operation in two different sets of flight data: IMU, barometer, and
magnometer; and GPS and heading data.
These equations are used for state estimation:
where 𝜔 𝑚 is the rotation measurement from the IMU, 𝑎 𝑚 is the acceleration measure from the
IMU. A is the matrix
0
0
𝑔
where g is acceleration due to gravity. 𝜔 𝑏 is the gyro bias. 𝑎 𝑠 is the
acceleration scale, and ℎ 𝑏 is the barometer bias.
E is the output error. B is the magnetometer reading. The difference between observer and system is
given by:

8. 8
And they can be calculated by:
The output error then becomes this for the observer
The terms are then linearized about identity - (1 0 0 0 1 0)
It gives the following linear matrices:

9. 9
Where M is the system covariance matrix and N is the input covariance matrix.
The predicted covariance are used to calculate the Kalman gain
Pseudocode:
1. Read measurements
2. Predict the state based on trapezoidal integration of derivatives given by the state equations
3. Integrate all states in the body frame before transforming them to the world frame to avoid
coning and sculling errors
𝑥 𝑑𝑜𝑡 = 𝑓(𝑥, 𝑢)
4. Linearize the system with:
Δ𝑥 𝑑𝑜𝑡 = (𝐴 − 𝐾𝐶)Δx − Mw + KNv
5. Predict covariance by integrating:
𝑃𝑑𝑜𝑡 = 𝐴𝑃 + 𝑃𝐴 𝑇
+ 𝑀𝑀 𝑇
− 𝑃𝐶 𝑇(𝑁𝑁 𝑇)−1
𝐶𝑃
6. Compute Kalman gain using the equation:
𝐾 = 𝑃𝐶 𝑇(𝑁𝑁 𝑇)−1
7. Use the Kalman gain to update the state and covariance via integrating:

10. 10
𝑥 𝑑𝑜𝑡ℎ𝑎𝑡
= 𝑓(𝑥ℎ𝑎𝑡, 𝑢) + 𝐾(𝑦 − ℎ(𝑥ℎ𝑎𝑡, 𝑢))
𝑃𝑑𝑜𝑡ℎ𝑎𝑡
=(I-KC)*P
Results:
For the recorded quadcopter flight data sets, the MEFK predicted state did not converge due to
inconsistency in knowledge of the initial sensor noise and system noise matrices. The data had noise,
but the trajectory was one that an IEKF or GMEKF should be able to handle. Based on the literature,
these kinds of filters show very good convergence to several trajectories. Therefore, there is likely some
error in our code. For future studies the significance of quaternion geometry would be useful to view.
The literature on using quaternions is opaque. The documentation for the generalized MEKF is not
necessarily complete – it only covered simulation and arbitrarily chosen its system and input covariance
error matrices. It was also a surprise to discover that these filters were meant for observer-based
systems, rather than being directly applicable to a quadcopter. However, this project did lead to a better
understanding of the filtering methods available for a quadcopter.
References:
1.Baker, Martin John. "Quaternion Differentiation." Http://www.euclideanspace.com/. N.p., n.d. Web. 7
Dec. 2016.
2.Bonnabel, Silvère, Philippe Martin, and Pierre Rouchon. "Symmetry-preserving observers." IEEE
Transactions on Automatic Control 53.11 (2008): 2514-2526.
3.Bonnabel, Silv`ere , Philippe Martin, Erwan Sala¨un. Invariant Extended Kalman Filter: theory and
application to a velocity-aided attitude estimation problem. 48th IEEE Conference on Decision
and Control, Dec 2009, Shanghai, China. pp.1297-1304, 2009.
6.Pittelkau, Mark E. "Rotation vector in attitude estimation." Journal of Guidance, Control, and
Dynamics 26.6 (2003): 855-860.
4.Bonnable, Silvère, Philippe Martin, and Erwan Salaün. "Invariant Extended Kalman Filter: theory and
application to a velocity-aided attitude estimation problem." Decision and Control, 2009 held
jointly with the 2009 28th Chinese Control Conference. CDC/CCC 2009. Proceedings of the 48th
IEEE Conference on. IEEE, 2009.
5.Gibiansky, Andrew. "Quadcopter Dynamics and Simulation." Http://andrew.gibiansky.com/. N.p., 23
Nov. 2012. Web. 07 Dec. 2016.
6."Gimbal Lock." Wikipedia. N.p., 14 Feb. 2009. Web. 7 Dec. 2016.

11. 11
7.Martin, Philippe, Erwan Sala¨un. Generalized Multiplicative Extended Kalman Filter for Aided Attitude
and Heading Reference System. 2010 AIAA Guidance, Navigation, and Control Conference, Aug
2010, Toronto, Canada. pp.AIAA 2010-8300, 2010.
8.Pittelkau, Mark E. "Rotation vector in attitude estimation." Journal of Guidance, Control, and
Dynamics 26.6 (2003): 855-860.
9. Roscoe, Kelly M. "Equivalency between strapdown inertial navigation coning and sculling
integrals/algorithms." Journal of Guidance, Control, and Dynamics 24.2 (2001): 201-205.
10. Aström, Karl Johan, and Richard M. Murray. Feedback systems: an introduction for scientists
and engineers. Princeton university press, 2010.