A strong tracking Extended Kalman Observer

for projectile attitude and position estimation

a* b* a*
Mohamed Boutayeb , Sébastien Changey , Juliana Bara
a
University of Louis Pasteur - Strasbourg, LSIIT-CNRS-UMR 7005, Bvd. S. Brant, 67400 Illkirch – France
b
Institut Franco-Allemand de recherches de Saint-Louis, BP 34, 68301 Saint-Louis – France

ABSTRACT

In this note we focus on convergence behavior of the Extended Kalman Filter used as a state estimator for
projectile attitude and position estimation. We provide first the complete dynamical model, into a non linear state space
form, to describe the projectile behavior. Due to strong non linearities and poor observability of the system, very few
estimation techniques could be applied, among them the celebrate EKF. The later is, however, very sensitive to bad
initializations and small perturbations. The main contribution of this work lies in the use of a modified EKF to assure a
strong tracking using magnetometer sensor only. The modified EKF follows from the connection of some instrumental
matrices, fixed by the user, and the convergence behavior. Simulation results show the good performances of the
proposed approach.
Keywords: Projectile attitude and position estimation - Observers Design - Extended Kalman Observer - Stability and
convergence analysis - Simulation results.

1. INTRODUCTION
In this contribution we analyze convergence behavior of a simple and useful design procedure used as a state
estimator of the projectile attitude and position. Based on our recent works, we show here the connection between some
instrumental matrices and the basin of attraction. During the last four decades, state estimation for non linear dynamical
systems has the subject of tremendous research activities. A large wide of applications concerns, in particular, the
military domain. In this note we focus on projectile attitude and position estimation using magnetometer sensor only.
Attitude is usually estimated by complete system with several sensors. According to important biases on rotations
sensors, it is essential to use a fix reference such as earth magnetic field. This paper investigates the comparison of the
direction of the earth measured on the projectile with the computation of the projection by the estimation of the attitude.
According to non linearity of the evolution and the observation model, only a Kalman type observer (namely EKO or
EKF in a stochastic context) can be used. However, it is worth to notice that the main drawback of this standard
technique is the extreme sensitivity to initializations or divergence when the system is poorly observable.
Furthermore, from the established sufficient conditions for asymptotic convergence we provide a simple way to
design such arbitrary matrices in the goal to ensure strong tracking in spite of very bad initializations and perturbations.
Performances of the proposed approach will be show through simulation results under worst conditions.
Let us note that attitude and position estimation by using magnetometer is a low cost solution to replace, in the
future, a GPS embedded on a projectile. Magnetometers are chosen according to there robustness to very high
acceleration and according to the fact that by using a fix reference (the direction of the earth magnetic field), biases and
derives could be corrected.

2. DYNAMICAL MODEL
In this section we describe the projectile behavior from ballistic equations. All the system differential equations and the
magnetometer sensor measurements are written into the non linear state space form.

2.1 Sensor and projectile description

A three-axis magnetometer sensor is embedded on a projectile to measure the projection of the Earth magnetic field.
Below, Figure 1 shows the three-axis sensor orientation and the direction of the Earth magnetic field.

Sensors, and Command, Control, Communications, and Intelligence (C3I) Technologies for Homeland Security
and Homeland Defense VI, edited by Edward M. Carapezza, Proc. of SPIE Vol. 6538, 65381G, (2007)
0277-786X/07/$18 · doi: 10.1117/12.718993

Proc. of SPIE Vol. 6538 65381G-1

Downloaded From: http://proceedings.spiedigitallibrary.org/ on 07/03/2016 Terms of Use: http://spiedigitallibrary.org/ss/TermsOfUse.aspx

 η ,θ α , β , ϕ 2 = ϕ1 + ψ
r r r r r r r r r
(i , j , k ) (t , s , h ) (c , a , b )
çeart Earth Velocity Projectile

Figure 2 : 5 angles needed

Figure 1 : Three-axis sensor

According to Euler's rotation theorem, any attitude may be described by three angles. By this global representation, it is
not possible 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 :
r r r
• η , θ define the orientation of the velocity vector in the "Earth frame" i , j , k
r r r r r r
• α , β , ϕ 2 define attitude of the "body frame" c , a , b in the "velocity frame" t , s , h

r
s r
h
r r
j t r
s
θ
r r
h i' ≈β r
r s'
ϕ1
r i r
h η c
r α
β
r i' ψ r
s
k ϕ 2 = ϕ1 + ψ
r
ψ = arctan 2(− β ,α ) velocity V ≈α

Figure 3 : Trajectory frame definition Figure 4 : Attitude frame definition

On Figure 3, azimuthη and slope θ give a representation of velocity (trajectory). Then, on Figure 4, angle of attack α ,
side-slip angle β and roll angle ϕ 2 define projectile orientation around velocity. As α , β andη are in the range of few
milliradians, angles and sinus can be approximated: sin(α ) ≈ α , sin( β ) ≈ β and sin(η ) ≈ η . The roll angle
r
ϕ 2 = ϕ1 + ψ defines the rotation around t .
2.2 Mechanical Model
The dynamical model presented here follows from the complete mechanical equations of ballistic, projected in different
frames and using approximations owing to the small range of the evolution angles.
The equations are decomposed into three sub-models:
1. Attitude evolution:
⎧ϕ&&2 = − k ϕ& 2
⎨
⎩ξ ' '+(a1 − i b1 )ξ '+(a2 − i b2 )ξ = a3 − i b3 with ξ ' = VD ξ&

where:

Proc. of SPIE Vol. 6538 65381G-2

Downloaded From: http://proceedings.spiedigitallibrary.org/ on 07/03/2016 Terms of Use: http://spiedigitallibrary.org/ss/TermsOfUse.aspx

 • D is the diameter of the projectile;
• ai , bi , k depend on velocity v, altitude y, angular roll rate wc , azimuthη , slope θ and mechanical parameters;

• ξ is a complex variable, describing attitude, which can be approximated by α − iβ because on the range (mill
radians) of theses angles.;
• Fl , Fm , Fd are respectively lift force, Magnus force and drag force ; Fcη , Fcθ represent Coriolis force
component on η& , θ& equations.

2. Trajectory:
⎧
⎪η& = M v cos(θ) (β Fl + α Fm + Fcη )
1
⎪
⎪& 1
⎨θ = (− M g cos(θ) + α Fl − β Fm + Fcθ )
⎪ Mv
⎪ 1
⎪v& = (− M g sin(θ) − Fd )
⎩ M
3. Position:
⎧ x& = v cos(θ )
⎪
⎨ y& = v sin(θ )
⎪ z& = v η cos(θ )
⎩
& = F(X( t ), t ) with X = [X X ]T .
With all theses equations, a state space evolution model can be written: X 1 2

⎧⎪ X 1 = [η θ v ]T trajectory
⎨
[
⎪⎩ X 2 = α β α& β& ϕ 2 ϕ& 2 ]T
attitude

Trajectory and attitude evolution can be separated. So the non linear state space form becomes:
⎧⎪ X& 1 = F1 ( X (t ), t )
⎨&
⎪⎩ X 2 = F2 ( X (t ), t )

By decomposition of the real and imaginary part of the non-linear complex equation of the attitude, the evolution can be
written in matrix form. This form is more useful in the filtering algorithm.

X& 2 = A2 ( X (t ), t ) X 2 (t ) + B2 ( X , t )

⎡ α& ⎤ ⎡ 0 0 1 0 0 0⎤ ⎡ α ⎤ ⎡ 0 ⎤
⎢ β& ⎥ ⎢ 0 0 0 1 0 0⎥⎥ ⎢⎢ β ⎥⎥ ⎢⎢ 0 ⎥⎥
⎢ ⎥ ⎢ 2
⎢ α&& ⎥ ⎢− a2 vD2 − a1 vD 0 0⎥ ⎢ α& ⎥ ⎢a3 Dv2 ⎥
2 2
b2 Dv2 b1 Dv
⎢ && ⎥ = ⎢ ⎥⎢ ⎥+⎢ 2 ⎥
⎢ β ⎥ ⎢ − b2 D2
v2
− a2 Dv2 − b1 Dv − a1 Dv 0 0⎥ ⎢ β& ⎥ ⎢ b3 Dv 2 ⎥
2

⎢ϕ& 2 ⎥ ⎢ 0 0 0 0 0 1⎥ ⎢ϕ 2 ⎥ ⎢ 0 ⎥
⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥
⎢⎣ϕ&&2 ⎥⎦ ⎢⎣ 0 0 0 0 0 0⎥⎦ ⎢⎣ϕ& 2 ⎥⎦ ⎢⎣ 0 ⎥⎦
2.3 Observation Model
An embedded magnetometer sensor measures the projection of the Earth magnetic field along the axes of the projectile:
r r r
one sensor is along the axial direction c and the two other sensors are along radials directions a , b . The Earth

Proc. of SPIE Vol. 6538 65381G-3

Downloaded From: http://proceedings.spiedigitallibrary.org/ on 07/03/2016 Terms of Use: http://spiedigitallibrary.org/ss/TermsOfUse.aspx

 r r r r r r
magnetic field direction is known in the "Earth frame" i , j , k and it is measured in the "body frame" c , a , b . The
rotation matrix between these two frames represents attitude information [1]. As the sensors measure global attitude, the
observation equations written from the projection of the earth magnetic field on the body are composed by trigonometric
transformation 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 can be simplified:

⎡ H c ⎤ ⎡α (cos(θ) H j − sin(θ) H i ) − β H k + (− η cos(θ) H k + cos(θ) H i + sin(θ) H j )⎤

Y = ⎢⎢ H a ⎥⎥ = ⎢⎢ H 1 cos(ϕ 2 ) + H 2 sin(ϕ 2 ) ⎥
⎥
⎢⎣H b ⎥⎦ ⎢⎣ H 2 cos(ϕ 2 ) − H 1 sin(ϕ 2 ) ⎥⎦
with
⎧⎪H 1 = −α (cos(θ) H i + sin(θ) H j ) − sin(θ) H i + cos(θ) H j + η sin(θ) H k
⎨
⎪⎩H 2 = β (cos(θ) H i + sin(θ) H j ) + η H i + H k

3. EKF BASED STATE ESTIMATION

Owing to a large progress in microprocessor based systems, we consider here the problem of projectile attitude
and position estimation using the EKF technique on the discrete-time model i.e estimation of the state vector Xk at time
instant kT. The discrete-time model is obtained from the Euler (for X1) and Bilinear (for X2) discretization methods with
T = 0.001 s :
⎧⎪X 1( k +1) = X 1k + T F1 (X k , k ) ⎧⎪A 2 d = (2 I − T A 2 )−1 (2 I + T A 2 )
⎨ with : ⎨
⎪⎩X 2( k +1) = A 2d X 2 k + B 2d ⎪⎩B 2d = (2 I − T A 2 )−1 B 2
3.1 Convergence analysis of the EKF

For the stability analysis, let us consider the general form of nonlinear discrete-time systems:

xk +1 = f (x k ,uk ) (1.a)
yk = h(xk ,uk ) (1.b)

n r p
xk ∈ R , uk ∈ R and yk ∈ R denote the state, input and output vectors at time instant k respectively. The
nonlinear maps f (x ,u ) and h(x ,u ) are assumed to be continuously differentiable with respect of x . The EKF
k k k k k

that we use here as a state observer is given by :

ˆx = ˆx + Kk +1 ek +1 (2)
k +1 k +1/k
Pk +1 = (I − Kk+1Hk+1 )Pk+1/k (3)
xˆk +1 /k = f ( xˆ k ,uk ) (4)
T
Pk +1/k = Fk Pk Fk + Qk (5)
T T −1
where Kk +1 = Pk+1/k Hk +1 (Hk +1 Pk+1/k Hk +1 + Rk +1 ) (6)
ek +1 = yk +1 − h( xˆk +1/k ,uk +1 ) (7)

Proc. of SPIE Vol. 6538 65381G-4

Downloaded From: http://proceedings.spiedigitallibrary.org/ on 07/03/2016 Terms of Use: http://spiedigitallibrary.org/ss/TermsOfUse.aspx

 ∂ f (x k ,uk )
F = F( xˆ ,u ) = (8)
k k k ∂x k
x = xˆ
k k

∂ h(x k+1,uk+1)
H = H ( xˆ ,u )= (9)
k+1 k +1/ k k +1 ∂x k+1
x = xˆ
k+1 k+ 1/ k

In what follows, we point out connection between convergence of the state observer (2)-(9) and the instrumental positive
definite matrices R and Q that represent covariance matrices in a stochastic context.
k k

First, we need to define x˜ and x˜ respectively by :

k +1 k +1 /k
x˜ = x − ˆx (10)
k +1 k+1 k +1
˜x
k +1 /k
= xk +1 − ˆx k+1/k (11)

and a candidate Lyapunov function Vk +1 as follows :

T −1
Vk +1 = x˜ k+1Pk +1 ˜xk +1 (12)
By subtracting both sides of (2) from x , we obtain :
k +1
x˜ = x˜
k +1
−K e
k+1/k k +1 k +1
(13)

instead of using the following classical approximations :

x˜k +1 /k ≈ Fk x˜k (14)

and ek +1 ≈ Hk+1x˜ k+1/k (15)

We introduce unknown diagonal matrices σk and τk, to model errors due to the first order linearization technique, so that
we obtain the following exact equalities:
~
x k +1 = σ k Fk ~
xk (16)
τ k +1e k +1 = H k +1 ~
x K +1 / k (17)

We notice that for linear systems σk and τk are identity matrices for all k. The goal, in what follows, is to determine
conditions for which {V } is a strictly decreasing sequence taking σk and τk into account.
k k =1,...

Next, from (3) and (6), we have:

T −1 T T −1
Kk +1 = Pk+1Hk +1Rk +1 = Pk +1/ k Hk +1 ( Hk +1 Pk +1/ k Hk +1 + Rk+1) (18)
−1 −1 T −1
and Pk +1 = Pk +1/k + Hk +1Rk +1Hk +1 (19)

Substituting (18) into (13) and (13) into (12), the Lyapunov function Vk +1 becomes:
T -1 T T -1 T -1
Vk +1 = x˜ k +1/k Pk +1 ˜xk +1/k − x˜ k +1/k Hk+1Rk+1ek+1 − ek+1Rk +1Hk +1 ˜xk +1/k
T -1 T -1
+ek +1 Rk +1 Hk+1Pk +1 Hk +1 Rk +1ek +1 (20)

and (19) into (20) :

Proc. of SPIE Vol. 6538 65381G-5

Downloaded From: http://proceedings.spiedigitallibrary.org/ on 07/03/2016 Terms of Use: http://spiedigitallibrary.org/ss/TermsOfUse.aspx

 T T -1 T T -1
Vk +1 = Vk+1/k + x˜ k+1/k Hk +1 Rk +1 Hk+1x˜ k+1/k − x˜k +1 /k Hk +1 Rk +1ek +1
T -1 T -1 T -1
− e k+1Rk+1Hk+1x˜ k+1/k + ek +1 Rk +1 Hk +1 Pk +1 Hk +1 Rk +1ek +1 (21)
T -1
with Vk +1/k = x˜ k+1/k Pk+1/k ˜xk +1/k (22)

From (16) and (17), (21) becomes :

Vk +1 = Vk +1 / k + e Tk +1 (τ k +1 R k−1+1 τ k +1 − τ k +1 R k−1+1 − R k−1+1 τ k +1 + R k−1+1 H k +1 P+1 H Tk +1 R k−1+1 )e k +1 (23)
On the other hand Vk+1/k may be written as:

Vk+1/k = ~
x k Fk σ k (Fk Pk Fk + Q k ) σ k Fk ~ T −1 T T
xk (24)
A strictly decreasing sequence {V } means that there exists a positive scalar 0 < ζ < 1 so that :
k k =1,...
V − V ≤ − ζV (25)
k +1 k k

or equivalently
V − (1 − ζ )V = = e Tk +1 (τ k +1 R k−1+1 τ k +1 − τ k +1 R k−1+1 − R k−1+1 τ k +1 + R k−1+1 H k +1 P+1 H Tk +1 R k−1+1 )e k +1
k +1 k
x (F σ (F P F T + Q ) −1 σ F T − (1 − ς)P −1 )~
~
k k k k k k k kxT ≤ 0
k k k (26)

The main result is stated by the following theorem:

Theorem
If we assume that:
i) There exist positive real numbers η1 and η2 so that for all k ≥ M and for some finite M ≥ 0 we have :
T
η1I n ≤ Oe (k − M,k)ℜ (k − M,k)Oe (k − M, k) ≤ η2 I n (27)

⎡ Hk− M ⎤
⎢ H F ⎥
with Ο e (k − M,k ) = ⎢
k− M+1 k− M
. ⎥ and ℜ(k − M,k) = diag (R −1 ,..., R −1 )
k− M k
⎢ . ⎥
⎢ ⎥
⎣ Hk Fk−1Fk−2 ...Fk −M ⎦
−1
ii) Fk, Hk are bounded matrices and Fk exists (28)
1/ 2
⎛ λ (R k +1 ) ⎞
iii) τ i ( k +1) − 1 ≤ ⎜⎜ ⎟
⎟ for i = 1, ..., p (29)
⎝ λ ( H P
k +1 k +1 / k H T
k +1 + R )
k +1 ⎠
1/ 2
⎛ (1 − ς)λ (Fk Pk FkT + Q k ) ⎞
iv) σ jk ≤ ⎜ ⎟ for j = 1, ..., n (30)
⎜ λ ( F T )λ ( P )λ ( F ) ⎟
⎝ k k k ⎠
then lim (x k − xˆk ) = 0 (31)
k →∞

λ and λ denote the maximum and minimum singular values respectively. „

Proof
The proof follows from standard mathematical manipulations using the convergence condition (26).

Proc. of SPIE Vol. 6538 65381G-6

Downloaded From: http://proceedings.spiedigitallibrary.org/ on 07/03/2016 Terms of Use: http://spiedigitallibrary.org/ss/TermsOfUse.aspx

 Remarks
1 - The sufficient conditions (29) and (30) define, in fact, the neighbourhoods Ny and Nx of the actual trajectories

{y }k k=0 ,1,... {}
and xk
k= 0,1,...
in which the EKF converges.

2 - As σik and τjk are unknown parameters whose values depend on how far e and x˜ are from zeros, the bounds
k k
1/2 1/ 2
⎛ ⎛
( ) (1 − ζ )σ ⎛⎝ Fk Pk Fk + Qk ⎞⎠ ⎞
T
⎞
σ Rk +1
⎜ ⎟ and
⎜ ⎟ have to be as large as possible in order to
⎜ σ⎛H P
()
⎜ σ ⎛ FT ⎞ σ P σ ⎛ F ⎞ ⎟
T
H + R ⎠⎟ ⎞
⎝ ⎝ k +1 k+1/k k +1 k +1 ⎠ ⎝ ⎝ k⎠ k ⎝ k⎠ ⎠
satisfy (29) and (30).

3 - While the lower and upper bounds on τik, for any choice of Rk, are 0 and 2 respectively; i = 1, ..., p; those of σjk
1/ 2
⎛
(1 − ζ )σ ⎛⎝ Fk Pk Fk + Qk ⎞⎠ ⎞
T

depend on Qk which allows to obtain a very large value of

⎜ ⎟ . Indeed, since we have :
⎜ σ ⎛ FT ⎞ σ P σ ⎛ F ⎞ ⎟
⎝ ⎝ k⎠ k ⎝ k⎠ ⎠ ()
1/2 1/ 2
⎛ ⎛
( ) (1 − ζ )σ ⎛⎝ Fk Pk Fk + Qk ⎞⎠ ⎞
T
⎞
(1 − ζ )σ Qk
⎜ ⎟ ≤
⎜ ⎟
k
()
⎜ σ ⎛ FT ⎞ σ P σ ⎛ F ⎞ ⎟
⎝ ⎝ k ⎠ ⎝ k ⎠⎠ ⎝ ⎝ k⎠ ()
⎜ σ ⎛ FT ⎞ σ P σ ⎛ F ⎞ ⎟
k ⎝ k⎠ ⎠
1/2
⎛
it is easy to design Qk so that
⎜
(1 − ζ )σ Qk ( )
⎞
⎟ is large enough.
⎝ ⎝ k ⎠ k
()
⎜ σ ⎛ FT ⎞ σ P σ ⎛ F ⎞ ⎟
⎝ k ⎠⎠
On the basis of this remark the matrix Qk plays a central role to improve convergence of the EKF. A judicious choice of
T
Qk is : Qk = γek ek I n + δ In
where ek = yk − h(xˆk /k −1,uk ) .
γ have to be chosen sufficiently large, and positive, for bad initial conditions while δ is a positive scalar small enough.
The reason for this particular choice is to ensure the sufficient condition (30). Indeed, we notice that when xk is far from
1/ 2
⎛ ⎛ F P FT + Q ⎞ ⎞
T
(1 − ζ ) σ ⎝ k k k k⎠
ˆx , the term e e in Qk and then the upper bound ⎜ ⎟ is large, the value depends on the
⎜ σ⎛F ⎞σ P σ⎛ F ⎞ ⎟
()
k k k T

⎝ ⎝ k⎠ k ⎝ k⎠ ⎠
nonlinearities of f (x ,u ) , h(x ,u ) and the arbitrary positive scalar γ , and consequently we enlarge domain of
k k k k
T
attraction. On the other hand, when xk is close to ˆx , the term e e goes to zero and the proposed observer becomes the
k k k

classical EKF.

Proc. of SPIE Vol. 6538 65381G-7

Downloaded From: http://proceedings.spiedigitallibrary.org/ on 07/03/2016 Terms of Use: http://spiedigitallibrary.org/ss/TermsOfUse.aspx

 4. SIMULATION RESULTS
The algorithms described above are tested in simulation. A complete model of the projectile behavior generates various
signals of position x,y,z , velocity v and angles η ,θ ,α , β ,ϕ 2 . These angles let us compute the projections H c , H a , H b of
the earth magnetic field as if they were observed by the sensor on the projectile. Theses projections are provided to the
filter figure 5. So the results of the estimation can be compared to the reference (evolutions generated by the complete
model).

α , β ,ϕ2 noise

projections
η ,θ r + α̂
r H measure
H ref r
H ref
filter β̂
η ,θ

altitude y ϕ̂ 2
r
velocity v
Fig. 5. Simulation process

The algorithms are test using real evolution of attitude and position data for a shot with a 155 mm rotating projectile over
a distance of 16 km, with wind. (These data have been computed by data reduction algorithms to fit with externals
measures). First, the extended Kalman filter described in [3] has been tested and results show the method's limits. Then,
the new algorithm has been tested to compare the results.
4.1 Extended Kalman Filter

-4
η(t) (rad) θ(t) (rad) x 10
0.04 0.04 0.5 10
0.03 0.03
0.02 0.02 0 5
↑ error
→
0.01 0.01 ← error

0 0 -0.5 0
real
-0.01 -0.01
simulated
-0.02 -0.02 -1 -5
0 10 20 30 40 0 10 20 30 40

α(t) (rad) β (t) (rad) phi2(t)

0.01 0.01 6

0.005 0.005
4
0 0
2
-0.005 -0.005

-0.01 -0.01 0
0 10 20 30 40 0 10 20 30 40 20 20.005 20.01 20.015 20.02

shooting distance x(t) (km) altitude y(t) (km) velocity v(t) (m.s-1)
20 100 3 40 800 2
30 1
15
2 20 600 0
-1
∆ v en m.s
∆ x (m)

∆ y (m)

10 ← error 0 10 -1
← error ← error
1 0 400 -2
5
-10 -3
0 -100 0 -20 200 -4
0 10 20 30 40 0 10 20 30 40 0 10 20 30 40

Fig. 6. Estimation by EKF with perfect trajectory initialization

Proc. of SPIE Vol. 6538 65381G-8

Downloaded From: http://proceedings.spiedigitallibrary.org/ on 07/03/2016 Terms of Use: http://spiedigitallibrary.org/ss/TermsOfUse.aspx

 The complete movement of the projectile can be break down into two different movements. The attitude evolution is
described around trajectory evolution. As the magnetometer measure the complete movement, accuracy knowledge of
the trajectory at the beginning of the shot is needed. Extended Kalman filter described in [3] succeed in attitude and
trajectory estimation according to a very good initialization. For example, the estimations results presented on figure 6
are computed with a perfect initialization of the trajectory.
As the estimation shows good results, estimation and real evolution cannot be distinguished. So error is presented in
another range (on the right vertical axis). η̂ do not converge due to the wind ; the other estimation angles converge. For
the position integration, the errors are due to the wrong estimation of η̂ .

In view of real application, the same simulation has been run a second time with an initialization error about 20%. As the
results are presented on the figure 7, EKF do not converge at all. Estimation of attitudes angles αˆ , βˆ only converge at
the beginning of the flight. As trajectory anglesηˆ , θˆ and velocity do not converge, position integration diverges too.

η(t) (rad) θ(t) (rad)

2 4 1 1
1 3
0 2
-1 1 0 ↑ error 0
-2 ↑ error → 0
-3 real -1
simulated
-4 -2 -1 -1
0 10 20 30 40 0 10 20 30 40

α(t) (rad) β (t) (rad) phi2(t)

0.01 0.01 6

0.005 0.005
4
0 0
2
-0.005 -0.005

-0.01 -0.01 0
0 10 20 30 40 0 10 20 30 40 20 20.005 20.01 20.015 20.02

shooting distance x(t) (km) altitude y(t) (km) velocity v(t) (m.s-1)
20 1000 1000 800 60
3 40
15
2 500 600 20
← error -1
∆v en m.s
∆x (m)

∆y (m)

10 0 1 ← error 0
← error

0 0 400 -20
5
-40
0 -1000 -500 200 -60
0 10 20 30 40 0 10 20 30 40 0 10 20 30 40

Fig. 7. Estimation by EKF with 20% error on trajectory initialization

To improve attitude and trajectory estimation despite 20% error on trajectory initialization, a new method has been
tested.
4.2 EKF with a dynamic computation of Q
As the initialization error is about 20% on the trajectory, it is necessary to enlarge domain of attraction. According to
theorem demonstrated in previous part, a dynamic computation of state error variance matrix Q could improve the
results. So, this new method is tested in simulation. By using previous notations, the dynamic matrix Q could be written
Qk = γ ek ek I n + QEKF
T
as :

where QEKF is the same Q matrix used in previous simulation (γ = 0 ) .

Proc. of SPIE Vol. 6538 65381G-9

Downloaded From: http://proceedings.spiedigitallibrary.org/ on 07/03/2016 Terms of Use: http://spiedigitallibrary.org/ss/TermsOfUse.aspx

 Figure 8 shows estimation results by using the new Q matrix computation, always with 20% error on trajectory
initialization. By enlarging the attraction domain, filter converges whereas it didn't converge with classical EKF
estimation (γ = 0 ) .

η(t) (rad) θ(t) (rad)

0.2 0.2 1 0.1
real
0.15 simulated 0.1 0.5 0.05

0.1 0 0 0
↑ error

0.05 ↓ error → -0.1 -0.5 -0.05

0 -0.2 -1 -0.1
0 10 20 30 40 0 10 20 30 40

α(t) (rad) β (t) (rad) phi2(t)

0.01 0.01 6

0.005 0.005
4
0 0
2
-0.005 -0.005

-0.01 -0.01 0
0 10 20 30 40 0 10 20 30 40 20 20.005 20.01 20.015 20.02

-1
shooting distance x(t) (km) altitude y(t) (km) velocity v(t) (m.s )
20 10 3 60 800 2
40 1.5
15
2 20 600 1

-1
∆v en m.s
∆x (m)

∆y (m)
10 ← error 0 ↑ error 0 0.5
← error
1 -20 400 0
5
-40 -0.5
0 -10 0 -60 200 -1
0 10 20 30 40 0 10 20 30 40 0 10 20 30 40

Fig. 8. Estimation by new method with 20% error on trajectory initialization

To compare three simulations, results of the estimations are reported in the table figure 9.

perfect 20 % error 20% error

init init init
EKF EKF Modified
EKF
ηˆ − η (rad) 0.0005 0.5 0.1

θˆ − θ (rad) 0.04 0.2 0.01

αˆ − α OK NO OK

βˆ − β OK NO OK

ϕˆ2 − ϕ 2 OK NO OK

xˆ − x (m) 80 500 10

yˆ − y (m) 20 1000 40

vˆ − v ( m.s −1 ) 3 40 1.5
Fig. 9. Table of results

Proc. of SPIE Vol. 6538 65381G-10

Downloaded From: http://proceedings.spiedigitallibrary.org/ on 07/03/2016 Terms of Use: http://spiedigitallibrary.org/ss/TermsOfUse.aspx

 By comparison of the two last column of the table, we can see that attitude α , β , ϕ 2 converge despite 20%
ˆ
initialization error. θ converges too, with a better accuracy (10 mrad error instead of 200 mrad). By improving
estimation results on θˆ , position (x,y) estimation is improved too.
Azimuth angle η estimation is still a problem. To succeed in its estimation, it will be necessary to take into account the
wind in the Kalman filter.

5. CONCLUSIONS AND PERSPECTIVES

Attitude and position estimation seems to be possible in real time filtering. This paper presents a contribution which is
useful to make the estimation successful despite wrong initialization of the filter.
Forthcoming works will consist in a validation of the algorithm with measurements recorded on board the projectile,
instead of using simulated data. Before using on board data, a sensor model must be developed and integrate in the
algorithm.

REFERENCES

1. Fleck V., Introduction a la Balistique Extérieur, ISL, Coetquidan, 1998

2. Changey S., Modélisation et estimation par filtrage non linéaire, de l’attitude d’un projectile à partir de
magnétomètres, PhD Thesis, ISL/Supélec, 2005
3. Changey S., Fleck V., Beauvois D., Projectile attitude and position determination using magnetometer sensor only,
SPIE Defense and Security Symposium, Orlando, Florida, March 2005
4. K. Reif, F. Sonnemann, and R. Unbehauen. An EKF based nonlinear observer with a prescribed degree of stability.
Automatica, 34(9) :1119–1123, 1998.
5. Y. Song and J. W. Grizzle. Th extended Kalman filter as a local asymptotic observer for discrete-time nonlinear
systems. Journal of Mathematical Systems Estimation and Control, 5(1) :59–78, 1995.
6. J. P. Gauthier and G. Bornard. Observability for any u(t) of a class of nonlinear
systems. IEEE Trans. Automatic Control, 26(4) :922–926, 1994.
7. M. Boutayeb. Synchronization and input recovery in digital non-linear systems.
IEEE Trans. Circuits Syst. II, 51(8) :393–399, 2004.
8. M. Boutayeb and D. Aubry. A strong tracking extended Kalman observer for nonlinear discrete-time systems. IEEE
Trans. on Automatic Control, 44(8) :1550–1556, 1999.
9. D. H. Zhou, Y. X. Sun, Y. G. Xi, and Z. J. Zhang, “Extension of Friedland’s separate-bias estimation to randomly
time-varying bias of nonlinear systems,” IEEE Trans. Automat. Contr., vol. 38, pp. 1270–1273, Aug. 1993.
10. T. L. Song and J. L. Speyer, “A stochastic analysis of a modified gain extended Kalman filter with applications to
estimation with bearings only measurements,” IEEE Trans. Automat. Contr., vol. AC-30, pp. 940–949, Oct. 1985.
11. P. J. Galkowski and M. A. Islam, “An alternative derivation of the modified gain function of Song and Speyer,”
IEEE Trans. Automat.Contr., vol. 36, pp. 1323–1326, Nov. 1991
12. G. Ciccarella, M. Dalla Mora, and A. Germani, “A robust observer for discrete time nonlinear systems,” Syst.
Contr. Lett., vol. 24, pp. 291–300, 1995.

Proc. of SPIE Vol. 6538 65381G-11

Downloaded From: http://proceedings.spiedigitallibrary.org/ on 07/03/2016 Terms of Use: http://spiedigitallibrary.org/ss/TermsOfUse.aspx