KALMAN FILTER APPLIED TO GPS

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 Index

1. First approach to the Kalman Filter

2. Dynamic Systems: state-variable description

3. Kalman estimator in dynamic systems

4. Kalman in GPS: Extended Kalman Filter

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 1. KALMAN made simple

VV.VV
Vbattery vv.vv = Measurement=Vvoltmeter

V batt
≠V voltmeter
⇒ V =V +∈
volt batt
Error

∈ gaussian white noise, 0 mean, σm2. A ‘good quality’ Vbattery estimation

is needed:
* Defining v̂ = Battery voltage estimation

we apply the following algorithm

vˆ(n + 1) = vˆ(n ) + Correction

Instant n+1 Instant n
Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions
 KALMAN

Correction = K (n).[Vvoltimeter (n) − vˆ(n)] Reading

K (n) ?
K(n) is chosen (or constructed) with minimization criterion of
Quadratic Mean Error.

σ vˆ2 Noise variance

K ( n) = 2
σ vˆ + σ n2

Voltage estimation Variance

σ ( n) ?
2
vˆ
σ vˆ2 (n + 1) = (1 − K (n )).σ vˆ2 (n )

We assured to MINIMIZE ERROR VARIANCE

e = v − vˆ
σ e
2
= E e [ ] 2

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 KALMAN

vˆ(n )
Output
n -1 vˆ(n ) n
+ vˆ (n + 1) n +1
Σ
+
-
Σ x K(n+1)
+
Reading K(n)
vvolt(n)
σ vˆ2 (n)
K(n)
σ vˆ2 (n) + σ n2 (n)

1- K(n)

σ v2ˆ (n ) σ v2ˆ (n + 1)
x

Output
σ v2ˆ (n )

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 2. Dynamic systems

System: Phenomenon or physical process where its behaviour

(dynamic perfomance) can be described with the following
equations.
y1(t) Formulation (discrete case):
Dynamic System y2(t) State Eq. x k +1 = φ k x k + vk
x1(t), x2(t),.., xn(t)
ym(t)
Observation Eq.
y k = H k x k + wk

ü xk : state variables of Dynamic System

ü yk : measurement vector
ü Hk, φk : Observation matrix and state evolution matrix (both are LINEAR)
ü vk, wk: System noise and measurement noise.

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 State variables modeling

Ø Being u(t) the position of moving body at t instant where t >t0, then:

∂e(t ) 1 ∂e2(t ) 1 ∂e3 (t )

e(t ) ≈ e(to ) + (t − to ) + (t − t )2
+ (t − t )3
+ ...
∂t t =to 2! ∂t t =t 3! ∂t t =t
2 o 3 o
o o

position velocity acceleration jerk

Ø This n-order approach is correct if :

1 ∂e n +1 (t ) n +1
n +1
( t − t ) <ε
(n + 1)! ∂t
o
t =t o

Ø That depends on:

ü Dynamic characteristic of the mobile object.
ü Sampling interval

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 State variables modeling

considering t only up to second order terms

t = t o + ∆t
e (t o + ∆ t ) = e (t o ) + v (t o ) ⋅ ∆ t + 1 ⋅ a (t o ) ⋅ ∆ t 2 + altres
2
v (t o + ∆ t ) = v (t o ) + a (t o ) ⋅ ∆ t + altres
a (t o + ∆ t ) = a (t o ) + altres

Discretizing at k instants:

 e (k + 1)  1 ∆t ∆t 2   e( k ) 
2 
  
 v (k + 1) = 0 1 ∆ t  ⋅  v ( k )  + errors
 a (k + 1)  0 
1   a ( k ) 
   0

Due to truncation or system
These elements, called state variables, are the model extraction
vector of state variables

X k +1 = φ ⋅ X k + errors
k

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 State variables modeling

Ø How can we know the dynamic characteristic of mobile/dynamic

system? (state variables)

ü We have access to
à Measurement
For example: inside car Distance-meter/speedometer

 e(k ) 
Observable s =   = Z
 v (k ) 
Can we link measurement ßà state variables ?

Z = H ⋅ X + errors Observation errors

 e(k ) 
 e(k )   1 0 0   
Z =   =   ⋅  v (k )  + errors
 v ( k )   0 1 0   a(k ) 
  H = Observation matrix

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 State variables modeling + Kalman

X k +1 = φ ⋅ X k + W k
k Descripció mitjançant
Zk = Hk ⋅ X k +V k variables d’estat

Ø Assuming that we can access to different ZK, we can extract Xk

but noise, model errors (wK vK) etc. implies a degradation of Xk

Ø Can we estimate the course of the dynamic system?

State equation X k +1 ≅ φ ⋅ X k
k

Ø Can we estimate Xk minimizing the effect of errors and noise?

Kalman Filter (obviously...).

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 3. Kalman Filter

Being next S.D.

Formulation (discrete case):

Dynamic System
z1(t)
States Eq. x k +1 = Fk x k + v k
z2(t)
x1(t), x2(t),.., xn(t)
Observation Eq.
z k = H k x k + wk
zm(t)
v,w are whites, gaussians and their mean is 0
Lineal estimator without bias of minimum quadratic mean error will
be as :

ˆx k = xˆ −k + K k ( z k − H ⋅ xˆ −k )
where:
Ø x̂k is estimation of xk from z0, z1, .. , zk
x̂k− is a prediction from xˆ k −1

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 Kalman Filter

PROCEDURE

Ø We assume we get an estimation of the state variables

−
estimation prediction
(a prediction made at k-1 instant)
x̂ k instant

1. * We update with measures (observables)

There isn’t ‘-’ , It isn’t a prediction, but Update

−
xˆ k = xˆ k + correction [
K k ⋅ z k − H ⋅ xˆ k
−
]
Observation Eq.
We estimate the future position
−
xˆ k +1 = φ k ⋅ xˆ k
−
§ It’s necessary the first estimation x̂ o

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 Kalman Filter

Ø Què ha de fer l’algorisme?…

−
x̂ n −
x̂ n +1
Prediction
Algorithm
at n-1
Prediction
(Iteration n)

zn x̂ n
Observation
(measurement) Update

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 Kalman Filter

x n +1 = φ ⋅ x n + w n modeling errors

n
Es minimitza el seu efecte
zn = H n ⋅ x n + v n
Noise and
measurement errors

−
xˆ n UPDATE
PREDICTION x̂ n
PREDICTION
−
xˆ n +1

[
xˆ n = xˆ n− + K z n − H n ⋅ xˆ n− ]
ˆx n− +1 = φn ⋅ xˆ n

BASIC CALCULATIONS
MEASUREMENT zn (K,Q, R,P )
Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions
 Kalman Filter

Ø Assuming x0, {vk} & {wk} are random independent variables,

incorrelated and mean= 0,

E [w k w l ] = Rδ (k − l ) E [v k v l ] = Qδ (k − l )
Ø then :

− −
x k = x k + K k (z k − H k ⋅ x k )
ˆ ˆ ˆ
ˆx k− +1 = φ ⋅ xˆ k
( )
k −1 covariance Matrix
− − w →Q
K k = Pk ⋅Hk ⋅ Hk ⋅Pk ⋅Hk + R
T T
Of modeling errors
−
Pk = ( I − K k ⋅ H k ) ⋅ P k v →R Covariance M
Measurement noise
Pk−+1 = φ ⋅ P k ⋅ φ + Q
T
e k = x k − xˆ k covariance Matrix
k k k of the error
Pk

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 Kalman Filter

Initial Values ( xˆ 0 , P 0 , K 0 )
− zk INPUT

− −
xˆ k = xˆ k + K k (zk − H k xˆ k )
−
(
Kk = Pk ⋅Hk ⋅ Hk ⋅Pk ⋅Hk + R
T − T
)
−1

Pk = (I − K k Hk )Pk
−

−
Delay
xˆ k +1 = Φk xˆ k
−
x̂k OUTPUT
= φ ⋅ P k ⋅φ + Q
T
P k +1
k k k

Ø Computational cost per iteration : ~ n3

Ø if x0, vk and wk are gaussians, Kalman Filter is THE BEST POSSIBLE
ESTIMATOR

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 4. Kalman and GPS

A GPS Receiver can be seen as a…

Dynamic System measurements

pseudodistances

Dynamic System .
ρi , ρi
State Variables .
Yi, Yi
(position, velocity, accel., clock, …etc)
phase

(i=1.. n, where n is the no. of satèllites)

If the P, H, Q i R matrices can be extracted, Kalman filter will be

suitable for estimating its state variables.

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 Kalman and GPS

Ø Vector de variables d’estat per un usuari:

- Estàtic: ( x, y , z, b, b& ) (5 vars)

- de baixa dinàmica: ( x, x&, y , y& , z, z&, b, b& ) (8 vars)
- de dinàmica mitjana: ( x, x&, x&&, y , y& , y&&, z, z&, z&&, b, b& ) (11 vars)

Ø [Q] és funció de les imperfeccions del model de variables d’estats

ü Si dinàmica baixa per no considerar acceleració (i termes d’ordre
superior)
ü Si dinàmica mitjana el Jerk (i termes d’ordre superior)

& i les variables d’estat

Ø [H] ha de lligar ρ, ρ& , Ψ, Ψ

Ø [R] (matriu cov error de mesura) és funció de l’UERE (User Equivalent

Range Error) Error en la pseudodistancia

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 Kalman and GPS

& i les variables d’estat:

[H] ha de lligar ρ, ρ& , Ψ, Ψ

* Per exemple, si només r

(x2,y2,z2) (x3,y3,z3)
(x1,y1,z1)
(x4,y4,z4)
ρ2 ρ3
ρ1 ρ4

¿ (xu ,yu ,zu ,bu ) ?

bu ≡ cδ tu
ρ1 = ( x 1 − x u ) + ( y 1 − y u ) + ( z1 − z u ) + b u
2 2 2

ρ2 = ( x 2 − x u ) 2 + ( y 2 − y u ) 2 + ( z 2 − z u ) 2 + bu Sistema d’equacions
ρ3 = ( x 3 − x u ) 2 + ( y 3 − y u ) 2 + ( z 3 − z u ) 2 + bu NO-LINEAL !!!
ρ4 = ( x 4 − x u ) 2 + ( y 4 − y u ) 2 + ( z 4 − z u ) 2 + bu

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 Kalman and GPS

Ø Sigui el següent S.D. :

Formulació (cas discret):
z1(t)
Sistema Dinàmic z2(t) x k +1 = Φ k ( x k ) + v k
x1(t), x2(t),.., xn(t)
zm(t) y k = hk ( x k ) + w k
on Fk i/o hk són funcions NO-LINEALS.

Si es linealitzen les equacions del S.D. S’obté:

∆ x k +1 ≅ ϕ k ∆ x k + v k ∆x k = x k − x~k ; ∆zk = zk − z~k

amb:
x~ , z~ = valors nominals
k k

∆zk ≅ H k ∆x k + w k ∂ ∂
ϕk = ~ Φ k Hk = ~ hk
∂x ∂x
Valor nominal: un valor que nosaltres preestablim, proper a la solució.

En GPS la equació No-Lineal és la d’Observació

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions
 Kalman and GPS

En el nostre cas (GPS) i treballant només amb les pseudodistàncies:

Posició real xk
Posició nominal x~k
Pseudodistància real ρk
Pseudodistància nominal ρ~k ho és respecte cada satèl.lit i la
posició nominal x~
k
~
∆ ρ k = ρ~k − ρ k ρ~i k = ( x i − x~u k )2 + ( y i − y~u k )2 + ( zi − z~u k )2 + bu k

∆ x k = x~k − x k
~
∆bk = bk − bk
(xi,yi,zi) del satèl.lit i-essim
∆ ρ i k = ρ~i k − ρ i k ≅ a x i ∆ x u k + a y i ∆ y u k + a z i ∆zu k − ∆b
On axi,, ayi , azi son el cosinus directors del vector (unitari) des de xk
fins al satèl.lit i-essim
Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions
 Kalman and GPS

L’Equació d’observació queda com:

Matriu d’observació linealitzada Hk

 a x1 a x1 a x1 0 0 0 1 0
 
~  ax2 ay 2 az2 0 0 0 1 0
∆ρk = ρ k − ρk =  .∆ x k + w k
a ay 3 a z3 0 0 0 1 0
 x3 
 ax ay 4 az4 0 0 0 1 0 
 4

Aquestes columnes son ≠0 si considerem els observables ρ&

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 Kalman and GPS

ØL’algorisme trobarà la millor estimació de ∆x

ØPerò amb ella podrem obtenir x = x~ − ∆ x

~ , a partir d’ella es troba

ØCal determinar la posició nominal d’origen x 0
la posició en tot instant

ØL’equivalent de l’ample de banda del filtre és funció de les matrius Q i R

ØLa convergència de l’algorisme depen de la bondat de [H] (del pès del

vector d’error v) i això dependrà de la diferència entre:
( x, y , z ) i ( x~, y~, z~)
ØPot ocórrer que l’algorisme no convergeixi.

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions

 Bibliography

• E.D. Kaplan, Understanding GPS, Artech-House.

• K. Brammer, Kalman-Bucy Filters, Artech-House.
• R.G. Brown, Introd. To Random Signal Analysis and Kalman Filtering, Wiley
and Sons
• K. Deergha, An approach for a faster GPS Traking Extended Kalman Filter,
• Journal of I.O.N, Vol 42, No. 4 Winter 1995
• J. Figueras, Sistema Integrat GPS-INS Per mòbils de baixa dinàmica en
entorns urbans, PFC de ETSETB 1995.
• O. Gavilán, Desarrollo de un simulador de sistemas de navegación por satélite
en entorno MATLAB, PFC de ETSETB 1997
• R. Diego, Estudio y realización de un sistema de navegación GPS/INS para
móviles ... , PFC de ETSETB 1999.

Grup. D’Enginyeria electromagnètica I fotònica Dept. Teoria del Senyal i Comunicacions