Professional Documents
Culture Documents
Institut Franco-Allemand de Recherches de Saint-Louis, BP 34, 68301 Saint-Louis - France
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.
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
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 ≈α
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:
• ξ 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
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
ˆ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)
∂ 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
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,...
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)
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)
⎡ 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 →∞
Proof
The proof follows from standard mathematical manipulations using the convergence condition (26).
{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
⎝ ⎝ 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.
α , β ,ϕ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
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
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.
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
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 :
0.1 0 0 0
↑ error
0 -0.2 -1 -0.1
0 10 20 30 40 0 10 20 30 40
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
To compare three simulations, results of the estimations are reported in the table figure 9.
αˆ − α 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
REFERENCES