Orientation Estimation by Means of Extended Kalman Filter, Quaternions, and Charts

An orientation estimation algorithm is presented. This algorithm is based on the Extended Kalman Filter, and uses quaternions as the orientation descriptor. For the filter update, we use measurements from an Inertial Measurement Unit (IMU). The IMU consists in a triaxial angular rate sensor, and an also triaxial accelerometer. Quaternions describing orientations live in the unit sphere of R . Knowing that this space is a manifold, we can apply some basic concepts regarding these mathematical objects, and an algorithm that reminds the also called “Multiplicative Extended Kalman Filter” arises in a natural way. The algorithm is tested in a simulated experiment, and in a real one.


I. INTRODUCTION
K NOWLEDGE about the mechanical state of a system is necessary in many engineering fields.The orientation of the system is an important part of this mechanical state.Fields like robotics, virtual reality, or vehicle navigation among others, could require knowledge of the orientation of a system for tasks like: -Controlling an Unmanned Vehicle.
-Knowing the orientation of a camera in a scenario.
-Knowing the heading of a vehicle in a navigation system.
-Transforming measurements taken in the vehicle reference frame to an extern reference frame.The problem presents two main issues that need to be addressed: • We need to choose an orientation descriptor.
• We need to choose an estimation approach.The orientation of a system is understood as the rotation transformation that relates two reference frames: the one whose orientation we are interested in, and the reference frame of the system with respect to which we want to express such orientation.It only makes sense to speak about one orientation with respect to another system.Knowing that an orientation is a rotation transformation, our issue is to choose the most convenient parameterization for this rotation transformation.The most used parameterizations are the Euler angles, and their analogous, Tait-Bryan angles, rotation vectors, rotation matrices, and unit quaternions.A fairly complete survey of orientation representations is given in [1].Unit quaternions have properties that make them preferable against the other parameterizations.Namely: 1) There are no singularities (we avoid the "gimbal lock", that is present in Euler angles).2) They describe the orientation in a continuous way (unlike axis-angle representation).3) Motion equations are linear with quaternions.4) They are determined by 4 parameters (in contrast with a rotation matrix, that needs 9 parameters).Because of these properties, unit quaternions have been the most widely used orientation representation since the early 1980s [2], and we also use them in this work.
The orientation estimation problem has been addressed using several approaches.In [3] it is provided a survey of methods for orientation estimation by far more complete than could be given in this work, and it would not make sense to repeat it here.The Kalman Filter with its nonlinear versions is the protagonist.But there is a major issue: unit quaternion do not live in the Euclidean space, where the Kalman Filter is defined.This fact leads to a variety of approaches in the application of this formalism.In particular, the known as "Multiplicative Extended Kalman Filter" is the method of choice because of its accuracy, its relative simplicity, its computational efficiency, and for being flexible to incorporate a great variety of measurements.However, there seem to be some aspects of it that are still not well understood.
The objective of this paper is to explore a new view point for the Extended Kalman Filter applied to the estimation of orientations represented by unit quaternions.Its final form is very similar to that of the "Multiplicative Extended Kalman Filter", but it gets rid of the probably tricky definition of the "reset" operation, and it arises the introduction of a new update called "chart update".Not being that different the structure of these two algorithms, it would not be unreasonable to rename this MEKF as "Manifold Extended Kalman Filter".
The algorithm developed here is designed to take measurements from an Inertial Measurement Unit (IMU) which returns acceleration, and angular velocity measurements.Yet, this design is easily modifiable in order to adapt it to other type of sensors.
The MEKF has been tested in a simulated experiment, together with the known Madgwick algorithm [4].It also has been implemented in a real system, and tested with a commercial IMU.
The remainder of the paper is organized as follows.In Section II, we introduce the main properties of unit quaternions.In Section III, we introduce the basic concepts of manifold theory that will be used in the algorithm development.In Section IV, we review the motion equations and measurement equations.In Section V, we present the developed equations for the state prediction.In Section VI, we present the developed equations for the measurement prediction.In Section VII, we present the developed equations for the filter updates.Section VIII displays the experimental results.Finally, we expose the conclusions, and we picture the future work pathways in Section IX.II.QUATERNIONS Quaternions are hypercomplex numbers with three different imaginary units {i, j, k} , and can be expressed as q = q 0 + q 1 i + q 2 j + q 3 k . ( They can also be expressed in a vectorial form as Quaternion product is defined by the Hamilton axiom which produces the multiplication rule Quaternions describing rotations can be built with a unit vector that defines the rotation axis, q , and the angle of rotation, θ , through Having this form, they satisfy the restriction This means that quaternions describing orientations live in the unit sphere of R 4 .This space has dimension 3, although its elements are determined using 4 parameters.We will use basic concepts of manifold theory to handle this kind of space.
III. BASICS OF MANIFOLD THEORY When dealing with the Kalman filter, the distribution of a random variable, x , is encoded by its mean, x , and its covariance matrix, P , defined as This can be done when our random variables are elements of an Euclidean space.But when a random variable is an element of a manifold our covariance matrix definition could lose sense.This is our case, where the random variable q − q , does not describe an orientation.Then we need to redefine our covariance matrix in a different way, but we can not change the form of the definition of the covariance matrix if we want to use the Kalman filter, because this precise form is used in its derivation.We will solve this problem by defining our covariance matrix in a different space.But first we will review some previous definitions: a) Definition.Manifold: A n-manifold, M n , is a topological space in which each point is locally homeomorphic to the euclidean space, R n .This is, each point x ∈ M n has a neighborhood N ⊂ M n for which we can define a homeomorphism f : N → B n , with B n the unit ball of R n .b) Definition.Chart: A chart for a topological space, M , is a homeomorphism, ϕ , from an open subset, U ⊂ M , to an open subset of the Euclidean space, V ⊂ R n .This is, a chart is a function with ϕ a homeomorphism.Traditionally a chart is expressed as the pair (U, ϕ) .

A. The Set of Charts
Assume we know the expected value of our distribution of quaternions, q .In such case, we can express any unit quaternion as q = q * δ , with δ another unit quaternion (unit quaternions together with their multiplication rule form a group).And then, we can define the set of charts The set of charts (9), is used in [5], but this work does not talk about charts, and what we call "chart update" is not applied.
In each chart, ϕ q , the quaternion q is mapped to the origin.As the space deformation produced in the neighborhood of the origin is small, being the variance small, the distribution in each chart will be similar to the distribution in the manifold.
The inverse transformations for these charts are given by ϕ −1 q (e q ) = q * 1 4 + e q 2 2 e q . (10) B. The Transition Map a) Definition.Transition map: Given two charts (U α , ϕ α ) and (U β , ϕ β ) describing a manifold, with Having the set of charts defined by (9), and having two charts centered in quaternions p and q , and related by p = q * δ , then our transition map takes the form

IV. MOTION EQUATIONS AND MEASUREMENT EQUATIONS
The state of the system is defined by an orientation, encoded by a unit quaternion q , and by an angular velocity measured in the reference frame attached to our system, given by a vector ω .The unit quaternion defines a rotation transformation that determines the orientation of the system.This transformation relates vectors (denoted as v ) expressed in a reference frame attached to the solid whose state we want to describe, with the same vectors (denoted as v ) expressed in an inertial reference frame in which the gravity vector is expressed as g = (0, 0, −1) .Thus, our rotation transformation will yield Using a rotation matrix, And using our unit quaternion, where this time v = ( 0 v ) , and q * is the complex conjugate quaternion, that being q a unit quaternion, it is also its inverse.

A. Motion Equations
Knowing what our quaternion means, we can write the motion equations for the random variables that we use to describe the state of our system: where τ is the process noise, associated with the torque acting on the system, and its inertia tensor.

B. Measurement Equations
This work uses an IMU as information source.We can write the measurement equations that relate the random variables describing the state of our system, with the random variables describing the measurements of our sensors as follows: where r a t is the noise in the accelerometer measurement, r ω t is the noise in the gyroscope measurement, and a t are nongravitational accelerations.

V. STATE PREDICTION
In this section we expose the evolution equations used to perform the prediction of the expected value of the state, and of its covariance matrix.

A. Evolution of the Expected Value of the State
Taking the expected value in equations ( 15) and ( 16), assuming the random variables q(t) and q ω t (t) = t t-∆t τ (τ ) dτ to be independent, and the expected value of the process noise, q ω t (τ ) , to be constant when τ ∈ [t − ∆t, t) , our differential equations are transformed into other ones whose solutions are

B. Evolution of the State Covariance Matrix
Since we need a covariance matrix with a form like (7), we will define the covariance matrix of the state in the set of charts defined by (9).In particular, for each filter update we will have an expected value for the unit quaternion describing the orientation, q , and a covariance matrix defined in the Euclidean space, R 3 , whose points are related with those of the unit sphere of R 4 through the chart ϕ q .The origin of R 3 is mapped with the q quaternion, and points around the origin represent quaternions in the neighborhood of q .
1) Differential Equations for our Charts: This result, and its derivation, is totally inspired and is almost equal to that which appears in [2].Having the definition for our charts in (8) and (9), we can find the differential equations for the δ quaternion using the differential equations for q , (15), and for q .And having the differential equations for the δ quaternion, we can find the differential equations for a point e = 2 δ δ0 on the charts: Note that, by the definition of the charts, the vector of random variables e t is expressed in the chart centered in the quaternion q t .For each instant, t , we have a quaternion q t defining the chart for that time.Then the differential equation ( 21) defines the evolution of the vector e that "travels" between charts.
2) Differential Equations for the Covariance Matrix: We define the covariance matrix for the state of our system by Notice that we do not write "∆e t " .By the new definition of the covariance matrix, the term e t can be interpreted as a displacement from the q t quaternion, which is mapped to the origin of R 3 by the chart.Note also that being the covariance matrix symmetric, we do not need to find the evolution of all its terms.We just need to find the evolution of the terms P ee , P eω , and P ωω .
We are looking for an estimation of the covariance matrix in t , using the information in t − ∆t .This is, we want to get P t|t-∆t from P t-∆t|t-∆t .For P ωω it is easy to find this relation.Assuming the random variables ω t-∆t and q ω t to be independents, If we had a function e t = f (e t−∆t ) , we could replace in (22), and perhaps obtain a relation similar to (23).But we are not able to find a closed solution for (21).However, we can find a differential equation for P t using this differential equation for e t .Starting from ( 22), with After replacing (21), assuming that higher moments are negligible compared to second-order moments, and remembering our assumption of independence of the random variables q and q ω t , and therefore, of e and τ , ( 25) and ( 26) can be approximated by 3) Evolution Equations for the Covariance Matrix: We are dealing with a system of inhomogeneous linear matrix differential equations.Generally, a system of this type is untreatable, but in our case the equations are sufficiently decoupled to be able to find a solution.
Given a solution for P ωω , we can find an approximate solution for P eω .And with this solution, we can find an approximate solution for P ee .Denoting Ω = [ ω t ] × we can write with Q ω t = Q ω 0 t , being Q ω 0 a constant matrix representing the process noise covariance per time unit.

VI. MEASUREMENT PREDICTION
In this section we expose the measurement equations used to perform the prediction of the expected value of the measurement, and of its covariance matrix.
A. Expected Value of the Measurement 1) Expected Value of the Gyroscope Measurement: Taking the expected value on (18), (31) 2) Expected Value of the Accelerometer Measurement: Taking the expected value on (17), knowing that the g t vector does not change, and assuming that the non-gravitational accelerations affecting our system, a t , does not depend on its orientation, Using the unit quaternion describing the orientation of our system, this relation takes the form And if we use the rotation matrix constructed from this unit quaternion,

B. Covariance Matrix of the Measurement
The measurement is related to the state by means of the measurement equations: We can approximate linearly the relationship around the expected values, x t , and r t , using a Taylor series the Jacobian matrices evaluated on the expected value of the random variables.Then, our prediction equation of the measurement covariance matrix takes the form 1) Gyroscope Block: The measurement equation for the gyroscope is linear.Using (18) and ( 35) we obtain 2) Accelerometer Block: In order of being consistent with the Kalman filter formulation, the acceleration term, a t , should be part of the noise in the measurement, since if it were not so, it should be part of the state.Then, measurement noise in our Kalman filter has two components: • r t : the main measurement noise.This noise comes from the sensor.• a t : non-gravitational accelerations acting on the system.
These accelerations obstruct the measurement of the g vector.
Recalling that we express the covariance matrix of the state in R 3 , and knowing that doing q t = q t in the manifold, is equivalent to do e t = 0 in this space, we will have for the accelerometer measurement equation: , and after some calculus, 3) Measurement Covariance Matrix: Assuming independence of all random variables involved in the measure, our prediction equation for the measurement is where • Q a t is the covariance matrix of the random variable a t .• R a t is the covariance matrix that describes the noise in the accelerometer measurement, which is modeled by the random variable r a t .• R ω t is the covariance matrix describing the noise in the gyroscope measurement, which is modeled by the random variable r ω t .

VII. UPDATE
Although the original Kalman filter algorithm just requires the Kalman update, the fact that our covariance matrix is expressed in a chart makes necessary the computation of a second update.

A. Kalman Update
The Kalman update is performed in the space where the covariance matrix is defined.This is, we do not perform the Kalman update in the manifold, but in the chart.Given the estimate of the covariance matrix of the state, P t|t-∆t , and the estimate of the measurement covariance matrix, S t|t-∆t , the optimal Kalman gain is computed as Given the gain, we can update the state distribution in the usual way: The new state distribution will be expressed in the chart centered on x t|t−∆t .In this chart, e t|t-∆t = 0 , but e t|t = 0 .

B. Manifold Update
In order to find the quaternion corresponding to the updated vector e t|t expressed in the chart, we must reverse the function ϕ q t|t-∆t (e t|t ) making use of the equation (10):

C. Chart Update
After the Kalman update, the new state distribution is expressed in the chart centered on x t|t-∆t .We must update the covariance matrix expressing it in the chart centered on the updated state, x t|t , so that our information is expressed as at the beginning of the iteration.In order of achieve this objective, we must use the concept of transition maps, that for our charts take the form of (11).Being non-linear this relation, we need to find a linear approximation: e p (e q ) ≈ e p (e q ) + ∂ e p ∂e q eq=eq ( e q − e q ) .(43) After differentiating our transition map and evaluating in e t|t , having identified the charts ϕ p = ϕ q t|t and ϕ q = ϕ q t|t-∆t , we find out Then, our update equations for the charts are VIII.EXPERIMENTAL VALIDATION In this section we present results obtained from a simulated experiment, and a real one.

A. Simulated Experiment
Our simulated experiment consists on the definition of a path, the extraction of simulated measurements, the processing of this measurements, and the evaluation of the algorithm performance.Only knowing the real state, we are able to define some metrics to measure the performance of our algorithm.Finally, we display a comparison of the algorithm developed in this paper, which will be called Manifold Extended Kalman Filter (MEKF), and the currently popular algorithm developed by Madgwick [4], whose code can be found in [6].
1) Experiment Setup: For testing our algorithm, we can think in a simple and intuitive simulation.Let us imagine that we can freely roam the surface of a torus, which is a manifold whose space can be described in R 3 by The torus of our simulation will have R = 0.2m , and r = 0.05m .We can define a path in the torus using a third parameter to set the other two: We will use the path defined by v φ = 1 rad/s and v θ = 3 rad/s , and we will travel the path around the torus 3 times.This path can be seen in Figure 1.
Fig. 1.Path followed on the torus in our simulation.Reference frames that define the orientation of the IMU can be observed.
Accelerations occurring in the IMU can be calculated by differentiating twice in (48)-(50) with respect to the t parameter, resulting Now, we can define a reference frame for each point of the path.The axis of the reference frame will have the directions of the vectors After making the derivatives in (56), and choosing the direction of the vectors so that z points outward the torus surface, our rotation matrix relating a vector measured in the IMU reference frame, with the same vector measured in the external reference frame will be Matrix (57) can be expressed as the product of 3 known rotation matrices: Recognizing these matrices in (58), and using (5), we can find out the quaternion describing the orientation of our reference frame: Having (59) we can obtain the q quaternion: where And with the q quaternion we can use (15) to get the angular velocity: With all, we can generate a succession of states q r ω r , and for each state simulate a measurement a m t ω m t .After that, taking only the succession of measurements, we can make a succession of estimations about the state q e ω e using the orientation algorithms, and then compare our estimation with the known real state q r ω r .2) Error Definition: We will evaluate the performance of the algorithm through the definition of two errors: First, defining g r = (R r ) T (0, 0, −1) T as the gravity vector measured in the real reference frame attached to our system, and g e = (R e ) T (0, 0, −1) T as the gravity vector measured in the estimated reference frame, we define The e g error in (64) is defined as the angle between the vectors g r and g e .Being the lowest error the better, this gives us a measure of how well the algorithm estimates the direction of a vector for which we have directly related measurements.
Second, we define q r i , and q r f as the initial and final quaternions describing the real orientation of the system in our simulation; and q e i , and q e f as the initial and final quaternions describing the estimated orientation of the system.Then, defining ∆ r , and ∆ e as the quaternions describing the rotation transformations that relates the initials and finals orientations by q r f = q r i * ∆ r , and q e f = q e i * ∆ e , we define The e θ error in (65) is defined as the angle of the rotation defined by the δ θ quaternion, which satisfies ∆ e = ∆ r * δ θ .Being the lowest error the better, this gives us a measure of how well the algorithm estimates the whole orientation, including heading, for which we do not have directly related measurements.This second error definition seems unnecessarily complicated.We could think in something like e θ = 2 arccos (q r f ) * * q e f , but if we start the simulation with an unknown orientation for the algorithm, this definition would lead to a different quaternion from the starting one.The e θ error would have a bias because of the ignorance of the initial heading.Our e θ error definition is independent of this initial heading ignorance.
3) Setting the Algorithm Values: a) Initialization: The simulation starts with a known orientation state defined by b) Characterization of Process Noise: The following values have been established: Still, after some testing, we found that the algorithm behaves similarly with other configurations, provided that they are not disproportionate.A more worked up algorithm would introduce dynamical values for this variables.c) Characterization of Measurement Noise: The Kalman filter requires in order to produce an unbiased estimation.
For the covariance matrices we will set and we will compare how the error behaves as a function of the magnitude of the noise in the measurement.

4) Simulation Results:
We will place our simulation in 4 different scenarios, whose details are displayed in table I, and that have been chosen according to the current possibilities.
Results of Test 1 and 2 are not shown since the errors produced are too large.This suggests that both a good processor (small sampling time) as a good sensor (small variance) are required.
For Test 3 and 4 the time evolution of error measurements are plotted in Figures 2 -5.In Figures 3 and 5 we observe how the error becomes smaller as we improve our IMU (decreasing σ 2 ), while having a good processor (small ∆t).In Figures 2 and 4 we observe how the error becomes smaller as we improve our processor (decreasing ∆t), while having a good IMU (small σ 2 ).In Figure 2 we observe that the MEKF increases its accuracy in time, due to it adds information about the state in each update.The faster it updates (less ∆t) the faster it reaches convergence.On the other hand the Madgwick algorithm does not adds information, which implies that it can not "learn" about the past, and it does not increases its accuracy in time.We also observe that after reaching a certain ∆t no improvement in accuracy is seen.
In Figure 3 we can confirm the same observation made in the paragraph above.But we also note that it seems to be a limit in the algorithms accuracy as a function of the sensor noise.It could be an interesting appreciation because it could mean that beyond a certain sensor quality, there would not be an appreciable improvement in the estimation.
In Figure 4 we can notice that the errors tends to increase over time.It is best appreciated for the Madgwick algorithm, and for low update frequencies with the MEKF.Probably it is not appreciated for higher update frequencies because we have not waited enough.This happens because we have no reference for orientation in the plane perpendicular to the gravity vector.If we want to have a complete non-biased estimation of the orientation we should add measurements from additional sensors as a magnetometer, or a camera.
In Figure 5 we again see the same behavior noticed in the previous paragraph.We also repeat our observation about the limit of the estimation accuracy as a function of the sensor noise made two paragraphs above.

B. Real Experiment
Our real experiment consists on the visual inspection of the returned information by our algorithm, and the one returned by the algorithm implemented in a commercial IMU. 1) Test Bed: The algorithm has been implemented in a real system.It has been used a board containing a MPU6050 sensor.The processing is performed in the ATmega328P chip contained in an Arduino board.In this system the sampling time turns out to be about (74) Figure 6 shows the assembled system, consisting in the MPU6050 sensor, the Arduino UNO, and a MTi sensor of Xsens.2) Experiment Results: We have described a series of movements with the system.The movements have been carried out in four phases.The dynamics of each phase has been more aggressive than that of the previous phase.We have tried to finish with the same orientation with which the system began.Both have been saved the sensors measurements and the estimated states which are returned by the algorithms.In Figures 7 -9 these data are shown.In Figure 7 we can see that both sensor acceleration measurements are very similar.This makes us think that the misalignment between the two sensors is small.
In Figure 8 we note that both systems return a similar estimation of the orientation when the dynamics is not too aggressive.However, after some aggressive moves, the algorithm presented in this paper has a fast convergence.We also note the bias in the heading estimation of both algorithms when we look at q 2 quaternion component.The initial and final orientation should be the same, but we have no reference for orientation in the plane perpendicular to the gravity vector.
In Figure 9 we note that the measured angular velocity is very similar to the estimated angular velocity.Perhaps we could accept the gyroscope measurement as the real angular velocity of our system.Maybe then we could get some advantage in processing speed, and therefore greater accuracy of our algorithm.But this is left for future research.

IX. CONCLUSIONS AND FUTURE WORK
We have successfully used basic concepts of manifold theory for estimating orientations using quaternions as descriptors.A similar algorithm to the known as "Multiplicative Extended Kalman Filter" naturally arises in applying these concepts without having to redefine any aspect of the Extended Kalman Filter.
The orientation has been estimated using measurements from an IMU, but the basic idea introduced in this work is applicable to any other type of sensor intended to estimate the orientation.
We have tested the algorithm in a real experiment and we have compared our estimation with the one given by a commercial IMU, finding that both orientation estimations are similar.This tell us that our algorithm works as expected.
We also have tested the algorithm in a simulation.We have compared the performance of our algorithm with the algorithm developed by Madgwick.The results suggest that the algorithm developed in this paper could achieve a better accuracy than the one achieved by the Madgwick algorithm.However we dare not say so, as there may be various sources of error that we have not considered: -The update frequency depends on the processor.In the ATmega328P chip, contained in an Arduino board, the Madgwick algorithm is 16 times faster than the MEKF.-The chosen path could lead to pathological behaviors because of its symmetry.-We have seen the result of just a path.These considerations lead us to the following issues that will be addressed in future work: • We will design a simulation with results based on the averaging of multiple trajectories, and free of pathological behaviors.• We will test the algorithm for various chart definitions.
• We will test its Unscented Kalman Filter version with the various chart definitions.• We will compare the MEKF with the MUKF, and with the Madgwick algorithm.
• We will study our sighting about the limit in the algorithm accuracy as a function of the sensor noise.

APPENDIX A STATE PREDICTION
In this appendix we present in greater detail the developments concerning the state prediction.
A. Evolution of the Expected Value of the State 1) Evolution in the Expected Value of the Angular Velocity: Taking the expected value in equation ( 16), 2) Evolution in the Expected Value of the Orientation: Taking the expected value in equation ( 15), Assuming the random variables q(τ ) and q ω t (τ ) to be independent,1 This differential equation has no general closed solution.But if we assume that the expected value of the process noise, q ω t (τ ) , is constant when τ ∈ [t − ∆t, t) , then we will have the matrix differential equation This differential equation has the solution q(t) = e Ω ∆t q(t − ∆t) .
We can express this solution using the quaternion product, as  (79) 2) Differential Equations for the Covariance Matrix: a) Evolution Equation for P ωω : Assuming the random variables ω t-∆t and q ω t to be independents, their covariance is null.In such case, T × .
Here we can see the consequences of treating a nonlinear system.The evolution in the covariance matrix P ee , which is composed by moments of second order, is affected by the higher moments of the distribution.To find the evolution equations of the covariance matrix we would need information about the moments of order 3 and 4.These may depend of moments of order higher than them.Knowing all the moments of a distribution would mean to know all statistical information.What we can assume and expect is that higher moments to be negligible compared to second-order moments.
To find the solution to the inhomogeneous differential equation, we use the variation of constants method: Identifying the terms in the differential equation we obtain the following relation: To solve this last differential equation, it is necessary to propose a continuous evolution equation for P ωω .The simplest option is the linear function with Q ω t = Q ω 0 t , being Q ω 0 a constant matrix representing the process noise covariance per unit of time.
Having defined this continuous evolution equation, (88) is transformed into Integrating (90), The constant C is determined by the initial conditions P eω (0) = C(0) .With this in mind, Knowing that we have the information at t − ∆t , and we want to update the information at t , our equation becomes Finally, knowing that calculating infinite sums would take a lot, we can truncate in the first term, and write b) Evolution Equation for P ee : In this case we have the homogeneous equation Ṗee (τ ) ≈ − Ω P ee (τ ) − P ee (τ ) Ω T , whose solution is Using the variation of constants method, Identifying terms, we deduce the relation After substituting the expression for P eω , integrate with respect to time, and truncating in the first term of the infinite sums, the solution we want is given by T ∆t e −Ω T ∆t .

APPENDIX B MEASUREMENT PREDICTION
In this appendix we present in greater detail the developments concerning the measurement prediction.The g t vector does not change.If we also assume that the accelerations affecting our system, a t , does not depend on its orientation, One might be tempted to try to find the expected value of the matrix R t written as a function of the q t quaternion, but then we would run into the problem of the computation of expected values such as E[q 2 1 ] or E[q 1 q 2 ] .These expected values are defined in the manifold, and are what we try to avoid defining covariance matrices in the charts.
What we seek is not the expected value of the rotation matrix, but something like the "expected transformation".Then, using the quaternion describing the orientation of our system, this expression must be equivalent to Let us note that ∂ δt ∂et ∈ R 4×3 .However, the term APPENDIX C UPDATE In this appendix we present in greater detail the developments concerning the filter update.

Fig. 6 .
Fig. 6.Real system composed of an Arduino Uno, a MPU6050 chip, and a MTi sensor of Xsens.

Fig. 9 .
Fig. 9.Estimated and measured angular velocity during the real experiment.

A. Expected Value of the Measurement 1 )
Expected Value of the Accelerometer Measurement:Taking the expected value on (17),