Proceedings of the 2006 American Control Conference ThB03.

3
Minneapolis, Minnesota, USA, June 14-16, 2006

A Mixed Extended-Unscented Filter for attitude estimation with

magnetometer sensor
Sebastien Changey, Dominique Beauvois and Volker Fleck

Abstract— A priori information given by the complete mod-

elling of the ballistic behavior (trajectory, attitude) of a projec-
tile is simplified to give a pertinent reduced evolution model.
A new Mixed Kalman filter based on Extended and Unscented
Kalman filter is designed to estimate 3 attitude angles from
measures of the magnetic field of the earth given by a three- fig. 1 : three-axis sensor
axis magnetometer sensor embedded on the projectile.
The algorithm has been tested in simulation, using realistic
evolution of attitude data for a shot with a 155 mm rotating
projectile over a distance of 16 km, with wind and measurement
noise. The results show that we can estimate milliradians with
non-linear equations and approximations, with good accuracy.

fig. 2 : 5 angles are useful

I. INTRODUCTION
A three-axis magnetometer sensor is embedded on a
projectile to measure the projections of the Earth magnetic
field. The figure 1 shows the three-axis sensor orientations
and the direction of the Earth magnetic field. According to
Euler’s rotation theorem, any attitude may be described by
three angles. By this global representation, it is impossible
to separate the orientation of the velocity vector from the
attitude of the projectile around this vector. Therefore a new
representation with 5 angles is necessary (figure 2) :
• η and θ define the orientation
 of
 the velocity vector in
−
→ −→ − →
the ”Earth Frame” i , j , k ;
• α, β and ϕ2 define attitude of the ”projectile
 
frame”
−
→ −
→ → − −
− → fig. 3 : attitude definition
c ,→
−
a , b in the ”velocity frame” t , → s, h .
This paper is focused on the attitude estimation of the
projectile around its velocity vector : only the three last
angles α, β, ϕ2 are to be estimated. As angles of attack the shot.
α and side-slip β are in the range of few milliradians, An embedded magnetometer sensor measures projections of
angles and sinus can be approximated : sin(α)  α and the Earth magnetic field along the axes of the projectile
sin(β)  β. The roll angle ϕ2 = ϕ1 + ψ defines the rotation : one sensor is along the axial direction − →c and the two
−
→
−
→
around t . Figure 4 describes the evolution of the attitude others measure magnetic field in radial directions −
→a , b . The
for a shot with a 155 mm, 45 kg rotating projectile over Earth
 magnetic
 field direction is known in the ”Earth frame”
−
→ −→ − →
a distance of 16 km, with wind measurement noise. The i , j , k and it is measured in the ”projectile frame”
 
−
→ −
→
c ,−
→
whole shot lasts 50 seconds from initial conditions defined
a , b . A rotation matrix between these two frames
as V0 = 684 m.s−1 , η0 = 1, 35o and θ0 = 25o . The
represents attitude information. Its exact expression, defining
oscillations of the projectile in a nutation and precession
dependance with the 5 angles used to describe attitude, is
movement around its velocity vector are clearly apparent on
given in [2].
figure 4.b. Temporal evolution of attitude angles α and β are
specified on 4.c. Figure 4.d is a zoom on the first second of
II. MECHANICAL MODELLING
This work was supported by ISL.
S. Changey and V. Fleck are with ISL, French-German research institute
of Saint-Louis, P.O. BOX 70034, FR 68301 Saint-Louis Cedex, France To estimate the attitude of the projectile from the magne-
changey@isl.tm.fr and fleck@isl.tm.fr tometer sensor measurements, a Kalman filter is designed.
D. Beauvois is with Supelec, Automatic Control Department,
3 rue Joliot Curie, 91192 Gif sur Yvette cedex, France This algorithm needs an evolution model of the attitude for
dominique.beauvois@supelec.fr the projectile.

1-4244-0210-7/06/$20.00 ©2006 IEEE 2892

Authorized licensed use limited to: BEIHANG UNIVERSITY. Downloaded on June 15,2021 at 10:33:15 UTC from IEEE Xplore. Restrictions apply.
 incidence
0.01

0.005

α (rad)
0

−0.005

−0.01

−0.01 −0.005 0 0.005 0.01

β (rad)

(a) schema (b) incidence (2 first secondes)

0.01
0.01

0.005 0.005

α (rad)
α (rad)

0 0

−0.005
−0.005

−0.01
−0.01
0 5 10 15 20 25 30 35 40 45 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
temps (s) temps (s)

0.01 0.01

0.005 0.005
β (rad)
β (rad)

0 0

−0.005 −0.005

−0.01 −0.01
0 5 10 15 20 25 30 35 40 45 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
temps (s) temps (s)

(c) α and β (d) α, β (zoom)

fig. 4 : evolution of the projectile’s attitude

A. Evolution evolution equations (state dimension n=6) :

The reduced model is designed from a priori analysis Ẋ = F (X(t), t)
(3)
of the behavior of the projectile. The non-linear complex = A(X(t), t) X(t) + B(X(t), t)
equations of ballistics are detailed in [2] :
with :  T
ϕ¨2 = −k ϕ˙2 (1) X(t) = α(t) β(t) α(t) ˙ β(t)˙ ϕ2 (t) ϕ2˙(t)
⎡ ⎤
0 0 1 0 0 0
 
ξ + (a1 − i b1 ) ξ + (a2 − i b2 ) ξ = a3 − i b3 (2) ⎢ ⎥
⎢ 0 0 0 1 0 0 ⎥
⎢ ⎥
with ξ  = D ˙ ⎢ ⎥
V ξ ⎢ −a2 V 2 0 ⎥
2 2

⎢ D b2 DV
2 −a1 DV
b1 DV
0 ⎥
A = ⎢ ⎢ ⎥
0 ⎥
where : V2 V2
⎢ −b2 D2 −a2 D2 −b1 D −a1 D 0
V V
⎥
• D is the constant diameter of the projectile ; ⎢ ⎥
⎢ 0 0 0 0 0 1 ⎥
• V is the velocity ; ⎢ ⎥
⎣ ⎦
• ai , bi and k are functions of velocity V , altitude y, 0 0 0 0 0 −k
angular rotation wc , angles η and θ, depending on ⎡ ⎤
mechanical parameters ; 0
⎢ 0 ⎥
• ξ is a complex variable, describing attitude, which ⎢ ⎥
can be approximated by α − i β because of the range ⎢ a3 V 22 ⎥
and B = ⎢⎢ D ⎥
V2 ⎥
(milliradians) of theses angles. ⎢ b3 D2 ⎥
⎣ 0 ⎦
By this way, real and imaginary parts of the mechanical
equation of ballistics can be written to obtain state space 0

2893

Authorized licensed use limited to: BEIHANG UNIVERSITY. Downloaded on June 15,2021 at 10:33:15 UTC from IEEE Xplore. Restrictions apply.
B. Observation
As the sensor measures global attitude, the exact expres-
sions of the observations (written from the rotation matrix
in [2]) are composed by trigonometric expressions of the
five considered angles. Because of the low amplitude of the
attitude angles (α, β) and because of the low amplitude of
η (by definition of frames), these expressions are simplified
to give :
T
Y = [Hc Ha Hb ] (4)
Hc = α {cos(θ) Hj − sin(θ) Hi )} − β Hk
+{−η cos(θ) Hk + cos(θ) Hi + sin(θ) Hj }
Ha = H1 cos(ϕ2 ) + H2 sin(ϕ2 )
Hb = H2 cos(ϕ2 ) − H1 sin(ϕ2 )
with
⎧ fig. 5 : Filter
⎪
⎪ H1 = −α {cos(θ) Hi + sin(θ) Hj )} − sin(θ) Hi
⎨
+cos(θ) Hj + η sin(θ) Hk
⎪
⎪ H2 = β {cos(θ) Hi + sin(θ) Hj }
⎩
+η Hi + Hk
where Hi , Hj and Hk are the known components of the (ai , bi ), altitude y, angular motion wc and aerodynamic
normalized Earth Magnetic Field at the location of the shot. coefficients (drag, etc ...).
The algorithm used to estimate attitude is presented on figure
C. Discretization and design model 5, with 2 subparts :
According to Shannon’s theorem and roll rate value (wc is
about 1500 rad.s−1 ), discretization step is chosen as Te = • mechanical computation subsystem calculates the coef-
1 ms. For Kalman filter design, state and measurements ficients ai and bi at each step of the algorithm ;
noise must also be introduced : • a Kalman filter is used in its extended form, unscented
⎧
⎨ X[k + 1] = Fd (X[k], k) + V [k] form or mixed form. The extended form is described in
= Ad (X[k], k) X[k] + Bd (X[k], k) (5) [1] and the two others are described below.
⎩
Y [k + 1] = Gd (X[k], k) + W [k]
W [k] is a white noise introduced on the measured signals. Its
variance matrix is chosen R = 2.10−7 I3X3 to match with
the noise characteristics observed on the real sensor outputs.
(there is no unit on R because Hc ,Ha and Hb represent
only the direction of the earth magnetic field). A. Unscented Kalman Filter
V [k] is a discrete white noise introduced to take into account
model errors on X[k + 1]. Its variance matrix Q is computed
as written in [1] to compensate a 2 % error on the evolution
The Unscented Kalman filter [7] is a new method to
model.
estimate state variables of a model based on non linear
 
Q = diag 0 0 2 10−8 2 10−8 0 5 10−9 rad2 .s−4 equations of evolution and observation. The results given by
(6) this algorithm are better from Extended Kalman filter when
the linearisation of the equations is to hard to be obtained.
III. FILTERING The principle of the algorithm is based on the creation and
A new Mixed Extended-Unscented Kalman filter is pre- the propagation of a set of points ; from the propagation of
sented. This algorithm is designed from an Extended Kalman these points, variance and covariance will be computed.
filter [1] and from an Unscented Kalman filter ([6] and [5]). We describe here a simple adaptation of the Unscented
As the filter focuses on α, β and ϕ2 estimation, η, θ, v and Kalman filter presented in [6] and [5].
y evolution are assumed to be known or well estimated by The idea is first to compute a set of points NA around
another system. X̂[k|k] with variance error S[k|k]. Then we compute the
In the state space representation, the evolution matrix A evolution of all these points in a set of prediction points
depends on six mechanical coefficients ai , bi and the velocity NB . The third step is the computation of the observation
V . All of them have very slow variations. According to points NC (figure 6). From NB and NC points, variance
the attitude dynamics, the evolution matrix A could be and covariance matrices can be computed in order to
considered as constant during the sample time : at each step calculate a ”Kalman” gain to weight the innovation for
of the algorithm, A is computed from velocity V , parameters the determination of the estimated state X̂[k + 1|k + 1].

2894

Authorized licensed use limited to: BEIHANG UNIVERSITY. Downloaded on June 15,2021 at 10:33:15 UTC from IEEE Xplore. Restrictions apply.
 fig. 6 : Unscented Kalman filter
fig. 7 : Mixed Kalman filter
√ 
1. XNA (i) = Xest + 2 √
n Sest i ,  i = 1...n
XNA (n + i) = Xest − 2 n Sest i , i = 1...n
2. (ai , bi ) computation B. Mixed Kalman Filter
3. Ad , Bd computation from X[k], ai , bi If the evolution equations are well linearised by Ex-
4. XNB (i) = Ad XNA (i) + Bd , i = 1...2 n tended Kalman filter algorithm, the estimation results with
1

2n Unscented Kalman filter are very closed from the results
Xpred = 2 n XNB (i) obtained with Extended Kalman filter ; so it is not necessary
i=1
5. YNC (i) = Gd (XNB (i)), i = 1...2 n to use unscented algorithm because of the needed long

2n computation time. The Mixed Kalman filter described here
Ypred = 21n YNC (i) only use unscented algorithm for the observation (figure 7).
i=1

2n 1. (ai , bi ) computation
1
6. SX = Q + 2n (XNB − Xpred )(XNB − Xpred )T 2. Ad , Bd computation from X[k], ai , bi

2n
i=1
3. Xpred = Ad Xest + Bd
1
7. SXY = 2n (XNB − Xpred )(YNC − Ypred )T 4. SX = Ad Sest ATd +√ Q 
i=1 5. XNB (i) = Xpred + 2 n SX i ,  i = 1...n
√
1

2n
8. SY = R + 2n (YNC − Ypred )(YNC − Ypred )T XNB (n + i) = Xest − 2 n SX i , i = 1...n
i=1 6. YNC (i) = Gd (XNB (i)), i = 1...2 n
9. K = SXY SY−1 
2n
10. Xest = Xpred + K (Yobs − Ypred ) Ypred = 21n YNC (i)
11. Sest = SX − K SY K T i=1
1

2n

√  7. SXY = 2n (XNB − Xpred )(YNC − Ypred )T

2 n Sest i is the ith column of the matrix P (square root i=1

2n
1
of 2 n Sest ). 8. SY = R + 2n (YNC − Ypred )(YNC − Ypred )T
P T P = 2 n Sest i=1
9. K = SXY SY−1
It is possible to verify that the variance of the XNA points 10. Xest = Xpred + K (Yobs − Ypred )
is equal to Sest : 11. Sest = SX − K SY K T

1

2n C. Results
S = 2n (XNA (i) − Xest )(XNA (i) − Xest )T
i=1 As the real evolution of the attitude of a projectile is very
2 n √
  √ T hard to obtain, we only can use simulation to validate the
1
= 2n 2 n Sest 2 n Sest
i=1
i i algorithm. So a complete model generates various signals to
2 n √
  √ T be provided to the filter (V , y, η, θ, Ha , Hb , Hc ) and attitude
= Sest i
Sest i angles for comparison with estimated variables.
i=1
= Sest The various signals obtained from the simulation model can
be considerated as a realistic evolution because they have
This algorithm will be tested in simulation. The results are been validated by comparaison with an indirect measure of
described below. the attitude during a real shot disturbed by wind.

2895

Authorized licensed use limited to: BEIHANG UNIVERSITY. Downloaded on June 15,2021 at 10:33:15 UTC from IEEE Xplore. Restrictions apply.
 0.01
data
0.005 extended
particular
0

α (rad)
−0.005

−0.01

−0.015

−0.02
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8
temps (s)

0.01

0.005

β (rad)
−0.005

−0.01

−0.015

−0.02
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8
temps (s)

fig. 8 : Simulation process fig. 9 : Extended vs Unscented filter with S1d

−4
x 10
1.2 0.01
extended for S1d data
1 particular for S1d 0.005 extended
particular
variance on β

0.8 0

α (rad)
0.6 −0.005

0.4 −0.01

0.2 −0.015

0 −0.02
0 0.005 0.01 0.015 0.02 0.025 0.03 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8
temps (s) temps (s)

0.012 0.01
extended for S10d
0.01 particular for S10d 0.005
variance on β

0.008 0
β (rad)

0.006 −0.005

0.004 −0.01

0.002 −0.015

0 −0.02
0 0.005 0.01 0.015 0.02 0.025 0.03 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8
temps (s) temps (s)

fig. 10 : Estimated variance on β with S1d and S10d fig. 11 : Extended vs Unscented filter with S10d

Figure 8 explains the simulation process which takes into This table lets us conclude that estimation results are similar
account noise measurements. when initial variance is very low (S1d ), which can be seen
Xest [0] and Σest [0] are computed assuming that initial roll on figure 9. On the other hand, Unscented filter presents
angle is a centered uniform variable in the range [0; 2π] and better results when initial variance is higher : d is about 12
that initial incidence angle is a centered random gaussian times lower than for the S10d case. Figure 11 illustrates this
variable,
√ whose standard √deviation can have 2 different result : unscented estimation quickly converges.
values S1d = (1o /3) or S10d = (10o /3). Figure 10 compares error variances on β computed by the
To compare convergence quality of the filters, a distance be- two filters, with S1d initial condition on the upper figure
tween estimated and real data can be define : d([n m])(rad2 ) and S10d on the lower figure. The two plots of Figure 10
compute the relative quadratic error of the attitude between show that, at the beginning of the shot, error variances on
n and m secondes : β are quite similar whatever the filtering method, for the
two chosen initialisations. So, there is no need of using a
1

m/Te
(α̂[k] − α[k])2 + (β̂[k] − β[k])2 Unscented filter for prediction : extended approximation is
d([n m]) = (7)
Te + m − n α̂[k]2 + β̂[k]2 enough. So a Mixed Kalman filter will give same results
k=n/Te
than a Unscented filter, with a low computing complexity.
By this way we first compare the extended and the un-
scented filter for the two different initials values S1d and We now compare the Unscented and the Mixed filter
S10d . The table below represents d[0.1 1] for the two filters in the same operating conditions : Figure 12 and 13 show
and for the two initialisations. that the Mixed filter converges as quickly as the Unscented
filter whatever initial error variance (S1d , S10d ). Figure 14
d([0.1 1]) S1d S10d confirms that error variance on β is the same with the 3
extended 0.0545 0.4097 filters. We have also tested the Mixed Kalman filter all along
unscented 0.0380 0.0327

2896

Authorized licensed use limited to: BEIHANG UNIVERSITY. Downloaded on June 15,2021 at 10:33:15 UTC from IEEE Xplore. Restrictions apply.
 0.01 0.01
data data
0.005 particular 0.005 particular
mixed mixed
0 0
α (rad)

α (rad)
−0.005 −0.005

−0.01 −0.01

−0.015 −0.015

−0.02 −0.02
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8
temps (s) temps (s)

0.01 0.01

0.005 0.005

0 0
β (rad)

β (rad)
−0.005 −0.005

−0.01 −0.01

−0.015 −0.015

−0.02 −0.02
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8
temps (s) temps (s)

fig. 12 : Unscented vs Mixed filter with S1d fig. 13 : Unscented vs Mixed filter with S10d
−3
−4 x 10
x 10 5
1.2 data
particular with S 1d 4 estimation
1 mixed with S1d error
3
extended with S1d
variance on β

0.8

α (rad)
2
0.6 1

0.4 0

0.2 −1

−2
0 0 5 10 15 20 25 30 35 40 45
0 0.005 0.01 0.015 0.02 0.025 0.03
temps (s)
temps (s)

−3
x 10
0.012 2
particular with S 10d
0
0.01 mixed with S10d
extended with S10d −2
variance on β

0.008
β (rad)

−4
0.006
−6
0.004
−8
0.002 −10

0 −12
0 0.005 0.01 0.015 0.02 0.025 0.03 0 5 10 15 20 25 30 35 40 45
temps (s) temps (s)

fig. 14 : Estimated variance on β with S1d and S10d fig. 15 : Mixed estimation with S10d

the shot, on figure 15. The estimation keeps a low level
error all along the shot, as in [1]. After 35 s the influence
of the wind can be observed on the attitude angles α and β.
By computing quadratic error d[0.1 1] results for S1d are
confirmed.
d([0.1 1])
unscented
mixed
d for Mixed filter, with S10d is lower than d for Unscented
filter. This can also be observed by looking carefully figure
13. This can be explain by the fact that in our version
of Unscented filter, the gain K is obtained through an
evaluation of SXY which uses the set of points NB resulting
of the propagation of the set of points NA without taking
into account any state noise contribution (eq. 7, 8). In the
Extended Kalman filter, this state noise contribution is taken
into account in the calculation of the prediction error variance
(eq. 4).
Conclusion : Mixed Kalman filter seems to be well adapted
for this estimation problem. It’s a more simple alternative of
the Unscented Kalman filter.
IV. PERSPECTIVES
The simulation process presented on figure 8 needs the
knowledge of the evolution data of η, θ angles, altitude y and
velocity v. It could be interesting to design a new filter which
could estimate these evolution data with no new measure.
S1d S10d R EFERENCES
0.0380 0.0327 [1] S. changey, V. Fleck and D. Beauvois, Non linear filtering for attitude
0.0794 0.0031 estimation with magnetometer sensor, ICNPAA, Timisoara, Roumania,
2004.
[2] V. Fleck, Introduction a la balistique exterieure, ISL, Coetquidan,
1998.
[3] D. Beauvois, Automatique Statistique - Theorie de Wiener - Theorie
de Kalman, Supelec, 1997.
[4] Brown and Hwang, Introduction to Random Signals and Applied
Kalman Filtering, John Wiley and Sons, 1997.
[5] S. Brunke, Nonlinear filtering and system iden-tification algorithms
for complex autonomous systems, University of Washington, 2001.
[6] S. Julier, J. Uhlmann and H. F. Durrant-Whyte, A new method for
the nonlinear transformation of means and covariances in filters and
estimators, IEEE Transactions on automatic control, vol. 45, no 3.,
March 2000.
[7] E. A. Wan and R. van der Merwe, The unscented Kalman filter
for Nonlinear estimation, Adaptive Systems for Signal Processing,
Communication and Control (AS-SPCC), IEEE, Lake Louise, Al-
berta, Canada, Oct, 2000.

2897

Authorized licensed use limited to: BEIHANG UNIVERSITY. Downloaded on June 15,2021 at 10:33:15 UTC from IEEE Xplore. Restrictions apply.