TEFO Lucrarea 6 Filtrul Kalman

1
Laborator TEFO
Lucrarea nr. 6

FILTRUL KALMAN

Filtrul Kalman este un instrument matematic puternic care joac un rol important n
grafica pe computer cnd vrem s reprezentm lumea real n sistemele de calcul.
De asemenea, filtrul Kalman este un bun estimator posibil pentru o clas larg de
probleme.

1. Modele n spaiul strilor

Modelele n spaiul strilor sunt, n esen, o convenie de notaie pentru problemele de
estimare i control, dezvoltate pentru prelucrri matematice mai uoare. Se consider un
proces descris printr-o ecuaie cu diferene de ordin n (similar unei ecuaii difereniale) de
forma

1 0, 1, 1
, 0
i i i n i i n i
y a y a y u i
+ +
= + + + (1)

unde { }
i
u este un proces de tip zgomot alb i funcia de autocorelaie

( )
,
i j u i ij
E u u R Q = = (2)

unde E reprezint operatorul de mediere statistic, iar valorile iniiale { }
0 1 1
, , ,
n
y y y
+
sunt
variabile aleatoare de medie zero cu o matrice de covarian de dimensiuni n n cunoscut

( ) { }
0
, , , 0, 1
j k
P E y y j k n

= . (3)

Presupunnd zgomotul statistic independent de procesul care urmeaz a fi estimat, rezult:

( )
, 0
i j
E u y = pentru 1 0 n j + i 0 i , (4)

ceea ce asigur c

( )
, 0
i j
E u y = , 0 i j . (5)

Aceast ecuaie cu diferene poate fi rescris ca

[ ]
[ ] [ ]
[ ]

1 0 1 2 1
1
1 1 2
2 1
1
1 0 0 0 0
0 1 0 0 0
0 0 1 0 0
i
i i n n
i i
i i i i
i n i n
G
A x
y y a a a a
y y
x y y u
y y
+

+
+ +



= = +

(6)
TEFO Lucrarea 6 Filtrul Kalman

2

care conduce la modelul n spaiul strilor:


[ ] [ ][ ] [ ]
1 i i i
x A x G u
+
= + (7)
[ ][ ] 1 0 0
i i
y x = (8)

sau n forma mai general

[ ] [ ][ ] [ ]
1 i i i
x A x G u
+
= + , (9)
[ ][ ]
i i i
y H x = , (10)

unde [ ]
i
H reprezint matricea care descrie dependena liniar a ieirii de starea sistemului.
Ecuaia (9) reprezint dependena strii [ ]
1 i
x
+
att de starea anterioar [ ]
i
x , ct i de
un proces de zgomot
i
u . Ecuaia (10) descrie depedena dintre observaiile
i
y i starea intern
[ ]
i
x . Ecuaiile (9) i (10) sunt adesea numite modelul de proces, respectiv modelul de
msurare. Descrierea n spaiul strilor a sistemului descris de relaiile (9) i (10) este dat n
figura 1, unde se observ intrarea
i
u , starea [ ] x la momentele i i i+1, precum i ieirea
i
y a
sistemului.













2. Problema proiectrii observatorului

Exist o problem general legat de domeniul teoriei sistemelor liniare numit
problema proiectrii observatorului. Problema de baz este de a determina (estima) strile
interne ale unui sistem liniar, avnd acces numai la ieirile sistemului. Acest lucru este nrudit
cu problema cutiei negre unde este acces la unele semnale venind din cutie (ieirile), dar nu
se poate observa direct ceea ce este n interior.
Multe abordri la aceast problem de baz sunt tipic bazate pe modelul n spaiul
strilor prezentat n seciunea anterioar. Tipic exist un model de proces care modeleaz
transformarea strii procesului. Acesta poate fi, de obicei, reprezentat ca o ecuaie cu diferene
stocastic liniar similar cu ecuaia (9):

[ ] [ ][ ] [ ][ ] [ ]
1 k k k k
x A x B u w

= + + , (11)

[G] z
-1
[A]
[H
i
]
i
u [ ]
1 i
x
+

[ ]
i
x
i
y
Figura 1. Descrierea n spaiul strilor a sistemului descris de relaiile (9) i (10)
TEFO Lucrarea 6 Filtrul Kalman

3
unde [ ] A i [ ] B descriu dependena strii curente de starea anterioar, repsectiv de intrarea
anterioar.
n plus exist o form a modelului de msurare care descrie relaia ntre starea
procesului i msurrile efectuate. Acesta poate fi, de obicei, reprezentat cu o expresie
similar ecuaiei (10):

[ ] [ ][ ] [ ]
k k k
z H x v = + (12)

Zgomotul de proces [ ]
k
w i zgomotul de msurare [ ]
k
v sunt variabile aleatoare. Din
(10) i (12) se observ nlocuirea variabilei
k
y cu [ ]
k
z pentru a specifica faptul c msurrile
nu sunt neaprat strile specificate, ci pot fi orice combinaie liniar ale acestora.

Zgomotul de msurare i de proces
Zgomotul de msurare apare de exemplu n cazul msurtorilor cu ajutorul unui
senzor i datorit imperfeciunilor aparatelor de msur. n funcie de raportul semnal util-
zgomot trebuie s se in cont n estimare de informaia obinut din msurare.
Deoarece nici transformarea prin care se obine starea sistemului nu este perfect
trebuie s se in cont n estimaii strii de un zgomot de proces.

3. Filtrul Kalman

n continuare se descrie filtrul Kalman unde msurtorile i starea sunt considerate la
momente discrete n timp.

Procesul care urmeaz a fi estimat
Filtrul Kalman consider problema general a ncercrii de a estima starea [ ]
n
x a
unui proces controlat n timp discret care este guvernat de ecuaia cu diferene stocastic
liniar

[ ] [ ][ ] [ ][ ] [ ]
1 k k k k
x A x B u w

= + + , (13)

cu o msurare [ ]
m
z care este

[ ] [ ][ ] [ ]
k k k
z H x v = + . (14)

Vectorii de variabile aleatoare [ ]
k
w i [ ]
k
v reprezint i zgomotul de proces, respectiv,
zgomotul de msurare. Se presupune c zgomotul de proces
k
w i cel de msurare
k
v sunt
independente, de tip zgomot alb, i cu distribuii de probabilitate normale

[ ] ( ) [ ] ( )
0, p w N Q (15)
[ ] ( ) [ ] ( )
0, p v N R (16)

unde [ ] Q i [ ] R sunt matricele covarianei zgomotului de proces, respectiv a covarianei
zgomotului de msurare, presupuse a fi constante.
TEFO Lucrarea 6 Filtrul Kalman

4
Matricea [ ] A de dimensiuni n n n ecuaia cu diferene (13) leag starea la
momentul de timp anterior 1 k de starea la momentul curent k , n absena intrrii de
comand [ ]
k
u i a zgomotului de proces [ ]
k
w . Se face observaia c n practic [ ] A s-ar putea
schimba cu fiecare moment de timp, dar aici se presupune c este constant. Matricea [ ] B de
dimensiuni n l leag intrarea de control (opional) [ ]
l
u de starea [ ] x . Matricea [ ] H de
dimensiuni m n n ecuaia de msurare (14) leag starea [ ]
k
x de msurarea [ ]
k
z . n practic
[ ] H s-ar putea schimba cu fiecare moment de timp, dar aici se presupune c este constant.

Originile computaionale ale filtrului
Se definete

n
k x

(se observ super minusul) ca fiind estimatul strii a priori la
momentul k dat fiind cunotina procesului anterior la momentul k , i

n
k x

estimatul
al strii a posteriori la momentul k dat fiind msurarea [ ]
k
z . Se pot defini atunci erorile
estimailor a priori i a posteriori ca fiind

[ ]

k
k k
e x x

=



, i (17)
[ ] [ ]

k
k k
e x x

=

(18)

Covariana erorii estimatului a priori este atunci

{ }
T
k k k
P E e e

=

(19)

i covariana erorii estimatului a posteriori este

[ ] [ ][ ]
{ }
T
k k k
P E e e = (20)

n obinerea ecuaiilor pentru filtrul Kalman, scopul iniial este gsirea unei ecuaii
care calculeaz un estimat al strii a posteriori

k x


ca o combinaie liniar dintre un estimat
a priori

k x

i o diferen ponderat ntre msurarea actual [ ]
k
z i o predicie a msurrii
[ ]

k H x

aa cum este artat n ecuaia (21):


[ ] [ ] [ ]

k k k
k k
x x K z H x

( (
(
= +

( (
'
(21)

Diferena [ ] [ ]

k
k
z H x

n ecuaia (21) este numit inovaia msurrii, sau reziduul.
Reziduul reflect diferena ntre msurarea actual [ ]
k
z i msurarea prezis [ ]

k H x

.
TEFO Lucrarea 6 Filtrul Kalman

5
Matricea [ ]
k
K de dimensiuni n m n ecuaia (21) este numit ctigul sau factorul
de amestec minimiznd covariana erorii a posteriori din (20). Aceast minimizare poate fi
realizat astfel: mai nti se substituie ecuaia (21) n definiia de mai sus pentru [ ]
k
e (relaia
(18)), apoi se nlocuiete relaia obinut n ecuaia (20), se realizeaz medierile indicate, se
deriveaz urma matricei rezultate n raport cu [ ]
k
K , se egaleaz acel rezultat cu zero, i apoi
se rezolv ecuaia n [ ]
k
K . O form a lui [ ]
k
K rezultat care minimizeaz ecuaia (20) este
dat prin

[ ] [ ] [ ] [ ] [ ]
( )
1
T T
k k k
K P H H P H R


= +

(22)

Din (22) rezult c atunci cnd covariana erorii de msurare [ ] R se apropie de zero, ctigul
[ ]
k
K , care multiplic reziduul, ia valori din ce n ce mai mari. Anume,

[ ] [ ]
[ ] [ ]
1
0
lim
k
R
K H

= (23)

Pe de alt parte, cnd covariana erorii estimatului a priori
k
P

se apropie de zero, ctigul
[ ]
k
K multiplic reziduul cu valori din ce n ce mai mici. Anume,

[ ]
0
lim 0
k
k
P
K

= (24)

Se poate conchide c atunci cnd covariana erorii de msurare [ ] R se apropie de zero,
msurarea actual este din ce n ce mai credibil, n timp ce msurarea prezis
[ ]

k H x

este din ce n ce mai puin credibil, iar cnd covariana erorii estimatului a priori
k
P

se
apropie de zero msurarea actual [ ]
k
z este din ce n ce mai puin credibil, n timp ce
msurarea prezis
[ ]

k H x

este din ce n ce mai mult credibil.

Algoritmul filtrului Kalman discret

Filtrul Kalman estimeaz un proces prin utilizarea unei forme de control cu reacie:
filtrul estimeaz starea procesului la un anumit moment de timp i apoi obine reacia n forma
msurrilor (zgomotoase). Astfel, ecuaiile filtrului Kalman se mpart n dou grupe: ecuaii
de actualizare n timp i ecuaii de actualizare a msurrii. Ecuaiile de actualizare n timp
sunt responsabile de proiecia nainte (n timp) a estimailor strii curente i a covarianei
erorii pentru a obine estimaii a priori pentru urmtorul moment de timp. Ecuaiile de
actualizare a msurrii sunt responsabile de reacie adic de incorporarea unei noi msurri
n estimatul a priori pentru a obine un estimat a posteriori mbuntit.
Ecuaiile de actualizare n timp pot fi de asemenea gndite ca ecuaii predictor, n timp
ce ecuaiile de actualizare a msurrii pot fi gndite ca ecuaii corector. ntr-adevr algoritmul
TEFO Lucrarea 6 Filtrul Kalman

6
de estimare final seamn cu cel al unui algoritm predictor-corector pentru rezolvarea
problemelor numerice aa cum este artat n figura 2.
Ecuaiile specifice pentru actualizrile n timp sunt prezentate mai jos n tabelele 1 i 2.












Tabelul 1: Ecuaiile de actualizare n timp ale filtrului Kalman discret

[ ]

[ ][ ] 1
1
k k
k
x A x B u

= +


(25)
[ ][ ][ ] [ ]
1
T
k k
P A P A Q

= +

(26)

Din nou notm cum ecuaiile de actualizare n timp n Tabelul 1 proiecteaz nainte
estimaii strii i covarianei de la momentul de timp 1 k la momentul k .

Tabelul 2: Ecuaiile de actualizare a msurrii ale filtrului Kalman discret
[ ] [ ] [ ] [ ] [ ]
( )
1
T T
k k k
K P H H P H R


= +

(27)

[ ] [ ] [ ]

k k k
k k
x x K z H x

( (
(
= +

( (
'
(28)
[ ] [ ] [ ][ ] ( )
k k k
P I K H P

=

(29)

Prima sarcin n timpul actualizrii msurrii este de a calcula ctigul Kalman, [ ]
k
K .
Urmtorul pas este de a msura de fapt procesul pentru a obine [ ]
k
z , i apoi de a genera un
estimat al strii a posteriori incorpornd msurarea ca n (28). Pasul final este de a obine
estimatul covarianei erorii a posteriori prin (29).

Dup fiecare pereche de actualizare n timp i de msurare, procesul este repetat cu
estimaii a posteriori anteriori pentru a proiecta sau a face predicia pentru noii estimai a
priori. Aceast natur recursiv este una din caracteristicele foarte atractive ale filtruului
Kalman fcnd implementrile practice mult mai fezabile dect (de exemplu) o
implementare a unui filtru Wiener care este proiectat pentru a opera pe toate datele direct
pentru fiecare estimat. Filtrul Kalman n schimb condiioneaz recursiv estimatul curent de
toate msurrile trecute. Figura 3 ilustreaz complet funcionarea filtrului, combinnd
diagrama din figura 2 cu ecuaiile din tabelele 1 i 2.
n figura 4 este dat diagrama bloc a sistemului, n ipoteza absenei intrrii de control,
modelul de msurare i structura filtrului Kalman discret.


Actualizare n timp
(Predicie)
Actualizare a msurrii
(Corecie)
Figura 2. Ciclul filtrului Kalman discret
TEFO Lucrarea 6 Filtrul Kalman

7

Parametrii filtrului i reglare

n implementrile reale ale filtrului, covariana zgomotului de msurare [ ] R este de
obicei msurat nainte de funcionarea filtrului. Msurarea covarianei erorii de msurare [ ] R
este posibil, deoarece se poate msura procesul n timpul funcionrii filtrului lund cteva
eantioane de msurare.
Determinarea covarianei zgomotului de proces [ ] Q este n general mai dificil cnd,
de fapt, nu exist posibilitatea de a observa direct procesul care se estimeaz. Uneori un
model de proces relativ simplu poate produce rezultate acceptabile dac se introduce destul
incertitudine n proces prin selecia lui [ ] Q . n acest caz se sper c msurrile procesului
sunt fiabile.




Actualizare n timp (Predicie) Actualizare a msurrii (Corecie)
1) Proiecteaz starea nainte

[ ]

[ ][ ] 1
1
k k
k
x A x B u

= +



2) Proiecteaz covariana
erorii nainte
[ ][ ][ ] [ ]
1
T
k k
P A P A Q

= +


1) Calculeaz ctigul Kalman
[ ] [ ] [ ] [ ] [ ]
( )
1
T T
k k k
K P H H P H R


= +


2) Actualizeaz estimatul cu msurarea
[ ]
k
z

[ ] [ ] [ ]

k k k
k k
x x K z H x

( (
(
= +

( (
'

3) Actualizeaz covariana erorii
[ ] [ ] [ ][ ] ( )
k k k
P I K H P

=























Figura 3. O descriere complet a funcionrii filtrului Kalman
A

z
-1
H


z
-1
A H

K
k
k
w
+
+
+
+ +
-
+
+
k
x
1 k
x


k
v
k
z k
e

k x

1 k x

k x

SISTEM DISCRET
MSURARE
FILTRU KALMAN DISCRET
Figura 4. Diagrama bloc a sistemului, modelul de msurare i structura filtrului Kalman discret
TEFO Lucrarea 6 Filtrul Kalman

8
n orice caz, dac exist sau nu o baz raional pentru alegerea parametrilor, adesea
poate fi obinut performan superioar (statistic vorbind) prin reglarea parametrilor filtrului
[ ] Q i [ ] R . Reglarea este de obicei realizat naintea estimrii, de regul cu ajutorul altui
filtru Kalman ntr-un process numit, n general, identificare de sistem.
n condiiile n care [ ] Q i [ ] R sunt constante, att covariana erorii [ ]
k
P , ct i
ctigul Kalman [ ]
k
K se vor stabiliza rapid i apoi vor rmne constante (vezi ecuaiile de
actualizare ale filtrului n figura 3). Dac acesta este cazul, aceti parametri pot fi pre-calculai
fie prin rularea filtrului premergtor, fie, de exemplu, prin determinarea valorii de stare stabil
a lui [ ]
k
P .

4. Exemple de estimare folosind o filtrare Kalman

Estimarea nivelului de curent continuu
Se presupune exemplul estimrii unui nivel de tensiune continu. Se presupune c exist
posibilitatea de face msurri ale tensiunii continue, dar msurrile sunt perturbate de un
zgomot de msurare alb cu valoarea RMS de 0.1 voli. n acest exemplu, procesul este
guvernat prin ecuaia cu diferene liniar

1 k k k
x x w

= + , (30)

cu o msurare
1
z care este

k k k
z x v = + (31)

Astfel n acest caz matricele devin scalari. Starea neschimbndu-se de la un pas la altul rezult
1 A = i neexistnd control asupra intrrii, rezult 0 u = . Msurarea cu zgomot este direct a
strii, aa c 1 H = .
Ecuaiile de actualizare n timp sunt


1 k k x x

= ,
1 k k
P P Q

= + ,

i ecuaiile de actualizare a msurrii sunt

( )
1
k
k k k
k
P
K P P R
P R

= + =
+


( )
k k k
k k
x x K z x

= +
( ) 1
k k k
P K P

=

Se presupune o varian a procesului foarte mic, de exemplu 1 5 Q e = . (S-ar putea
seta 0 Q = , dar presupunnd o valoare mic, dar diferit de zero, rezult mai mult
flexibilitate n reglarea filtrului dup cum se va demonstra n continuare). Se presupune c din
experien se tie c valoarea adevrat a tensiunii continue are o distribuie de probabilitate
TEFO Lucrarea 6 Filtrul Kalman

9
normal standard, aa c se ncepe cu presupunerea c tensiunea este 0. Cu alte cuvinte,
nainte de ncepere se seteaz

1 0 k x = .
Similar este nevoie de alegerea unei valori iniiale pentru
1 k
P

, anume
0
P . Dac este
absolut sigur c estimatul strii iniiale

0 0 x = a fost corect, se va seta

0
0 P = . Totui dat
fiind incertitudinea n estimatul iniial

0 x , alegerea
0
0 P = ar cauza faptul ca filtrul s
iniializeze i apoi totdeauna s cread c

0 k x = . Ca urmare, alegerea alternativ nu este

critic. S-ar putea alege orice
0
0 P i filtrul eventual ar converge. Filtrul va starta cu
0
1 P = .
Valoarea tensiunii continue s-a considerat 1 x = . S-au simulat 50 de msurri distincte
k
z care au avut erori distribuite normal n jurul lui zero cu o deviaie standard de 0.1.
Programul pentru generarea tensiunilor continue, cu zgomot i estimate este dat mai
jos:

% P_Kalman_ex1
clear;
clc;
close all;

% parametri initiali
n_iter = 50;
sz = n_iter; % dimensiunea vectorilor
x =1; % valoarea adevarata a tensiunii continue
z = x+0.1*randn(1,sz); % observatii (normale raportat la x,
sigma=0.1)

Q = 1e-5; % varianta procesului

xhat=zeros(1,sz); % estimatul a posteriori a lui x
P=zeros(1,sz); % estimatul erorii a posteriori
xhatminus=zeros(1,sz); % estimatul a priori a lui x
Pminus=zeros(1,sz); % estimatul erorii a priori
K=zeros(1,sz); % castigul sau factorul de amestec

R = 0.1^2; % estimatul variantei masurarii; a se schimba
pentru a vedea efectul

% ghicirea initiala
xhat(1) = 0.0;
P(1) = 1.0;

for k=2:n_iter
% actualizare in timp
xhatminus(k) = xhat(k-1);
Pminus(k) = P(k-1)+Q;

% actualizare a masurarii
K(k) = Pminus(k)/( Pminus(k)+R );
xhat(k) = xhatminus(k)+K(k)*(z(k)-xhatminus(k));
P(k) = (1-K(k))*Pminus(k);
TEFO Lucrarea 6 Filtrul Kalman

10
end

valid_iter = 1:n_iter;
figure(1)
plot(z,'k+');
hold on;
plot(xhat,'b-v');
hold on;
plot(valid_iter,x*ones(1,sz),'g-&');
axis([1 n_iter min(z) max(z)]);
legend('masurari','starea estimata','valoae adevarata');
xlabel('Iteratie')
ylabel('Tensiune')

valid_iter(1)=[];
figure(2)
plot(valid_iter,Pminus(valid_iter))
axis([1 n_iter 0 0.01]);
xlabel('Iteratie')
ylabel('(Tensiune)^2')

n prima simulare s-a fixat variana msurrii la ( )
2
0.1 0.01 R = = . Rezultatele sunt
date n figura 3.
5 10 15 20 25 30 35 40 45 50
0.75
0.8
0.85
0.9
0.95
1
1.05
1.1
1.15
1.2
Iteratie
T
e
n
s
iu
n
e
masurari
starea estimata
valoare adevarata

Figura 3. Simularea cu ( )
2
0.1 0.01 R = =

Cnd s-a considerat alegerea lui
0
P de mai sus, s-a menionat c alegerea nu a fost
critic ct timp
0
0 P deoarece filtrul eventual ar converge. n figura 4 s-a desenat valoarea
lui
k
P n funcie de iteraie. De la a 50-a iteraie, a fost stabilit din alegerea iniial de 1 la
aproximativ 0.0003 (Voli
2
).
n figurile 5 i 6 de mai jos se poate vedea ceea ce se ntmpl cnd R este crescut sau
sczut cu un factor de 100. n figura 3 s-a considerat c variana msurrii a fost de 100 de ori
mai mare (adic 1 R = ) aa c a fost mai lent de a crede msurrile.

TEFO Lucrarea 6 Filtrul Kalman

11
5 10 15 20 25 30 35 40 45 50
0
0.001
0.002
0.003
0.004
0.005
0.006
0.007
0.008
0.009
0.01
Iteratie
(
T
e
n
s
iu
n
e
) 2

-

P
k

Figura 4. Dup 50 de iteraii, alegerea covarianei erorii
k
P

de 1 a fost stabilit
la aproximativ 0.0003 (Voli
2
)

5 10 15 20 25 30 35 40 45 50
0.8
0.85
0.9
0.95
1
1.05
1.1
1.15
1.2
Iteratie
T
e
n
s
iu
n
e
masurari
starea estimata
valoare adevarata

Figura 5. Simularea cu 1 R = . Filtrul este mai lent n rspuns la msurri, rezultnd ntr-o varian
estimat redus
5 10 15 20 25 30 35 40 45 50
0.8
0.85
0.9
0.95
1
1.05
1.1
1.15
1.2
Iteratie
T
e
n
s
iu
n
e
masurari
starea estimata
valoare adevarata

Figura 6. Simularea cu 0.0001 R = . Filtrul rspunde rapid la msurri, crescnd variana estimat

TEFO Lucrarea 6 Filtrul Kalman

12
n figura 6 s-a considerat c variana msurrii a fost de 100 de ori mai mic (adic
0.0001 R = ) aa c a fost mai rapid de a crede msurrile zgomotoase.
Dei estimarea unei tensiuni continue este relativ direct, aceasta demonstreaz clar
funcionarea filtrului Kalman. n figura 5, n particular, filtrarea Kalman este evident cnd
estimatul apare considerabil mai neted dect msurrile cu zgomot.

Sistem de urmrire prin radar
n acest sistem impulsurile radar sunt trimise, iar semnalele returnate sunt procesate
prin filtrul Kalman pentru a determina poziia n urma manevrei obiectelor aeropurtate.
Ecuaiile cu diferene ale dinamicii n formularea spaiului strilor sunt:
[ ] [ ]
1
1
1
2
1
0 1 0 0 0 0
0 0 1 1 0 0 0
0 0 0 0 0
0 0 0 0 1 0
0 0 0 0 0 1 1
0 0 0 0 0
k
k k
k
T
w
x x
T
w

= +







Ecuaia de observaie n timp discret este dat de
[ ] [ ]
1
2
1 0 0 0 0 0
0 0 0 1 0 0
k
k k
k
v
z x
v

= +



unde
[ ]
. .
1 2
T
k k k k k k k
x r r U U

=



k
r = deplasarea vehiculului la momentul k
.
k
r = viteza de deplasare a vehiculului la momentul k
1
k
U = zgomotul de stare corelat cu manevra
k
= unghiul vehiculului la momentul k
.
k
= viteza unghiular a vehiculului la momentul k
1
k
U = zgomotul de stare corelat cu manevra
T este perioada de eantionare.
[ ]
1 2
T
k k k
w w w =

= reprezint secvenele de zgomot alb de medie zero i covariane
2
1
, respectiv
2
2
.
[ ]
1 2
T
k k k
v v v =

= reprezint secvenele de zgomot alb ale senzorului de medie zero
i covariane
2
r
, respectiv
2

.
[ ]
k
w i [ ]
k
v sunt necorelate.
= coeficientul de corelaie.
Filtrul de modelare pentru albirea zgomotului de manevr este dat de
1 1 1
1 1 k k k
U U w

= + ,
iar
1
k
U comand starea vitezei de deplasare a vehiculului (
.
k
r ) i
2 2 2
1 1 k k k
U U w

= +
TEFO Lucrarea 6 Filtrul Kalman

13
iar
2
k
U comand starea vitezei unghiulare a vehiculului (
.
k
).
Au fost folosite urmtoarele valori pentru matricea de covarian iniial ([ ]
0
P ),
zgomotul de proces ([ ] Q ), i zgomotul de msurare ([ ] R ) pentru a genera rezultatele
covarianei.
[ ]
2
2
2 2
2
1
2
1
0
2
2
2 2
2
2
2
2
0 0 0 0
2 0 0 0 0
0 0 0 0 0
0 0 0 0
0 0 0 2 0
0 0 0 0 0
r
r
r r
T
T T
P
T
T T

+



=




+




[ ]
2
1
2
2
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0 0
0 0 0 0 0
Q

=







[ ]
2
2
0
0
r
R

=



0.5 = , T = 5, 10, respectiv 15 s.
( )
2
2
1000
r
m =
( )
2
2
0.017rad

=
( )
2
2
1
103 3 =
2 8
2
1.3 10

=
Programul Matlab care d rezultatele pentru covariane i ctigul Kalman este dat n
continuare. Este artat convergena elementelor de pe diagonal ale matricei de covarian
pentru perioadele de eantionare (5, 10, 15 s) i de asemenea valorile ctigurilor Kalman
selectate.

% P_Kalman_ex2
clear;
clc;
close all;

% parametri initiali
n_iter = 16;
sz = n_iter;
TEFO Lucrarea 6 Filtrul Kalman

14
T=[5 10 15]; % perioada de esantionare (se schimba
ulterior cu 10, respectiv 15)
ro=0.5;
for i=1:3
x(:,:,1,i)=zeros(6,1); % starea initiala
A=[1 T(i) 0 0 0 0;
0 1 1 0 0 0;
0 0 ro 0 0 0;
0 0 0 1 T(i) 0;
0 0 0 0 1 1;
0 0 0 0 0 ro]; % matricea care leaga starea curenta
de cea anterioara

% variantele zgomotelor de proces (deplasare si unghi)
sigma1p=(103/3)^2;
sigma2p=1.3e-8;

w1=sqrt(sigma1p)*randn(1,sz); %
w2=sqrt(sigma2p)*randn(1,sz);
H=[1 0 0 0 0 0;
0 0 0 1 0 0]; % matricea care leaga secventa de
observatie de starea curenta

% variantele zgomotelor de masurare (deplasare si unghi)
sigmarp=1000^2;
sigmatp=0.017^2;

v1=sqrt(sigmarp)*randn(1,sz);
v2=sqrt(sigmatp)*randn(1,sz);
z(:,:,1,i)=H*x(:,:,1,i)+[v1(1); v2(1)]; % observatia
initiala

% starile si observatiile la momentele urmatoare de timp
for k=2:sz
x(:,:,k,i)=A*x(:,:,k-1,i)+[0;0;w1(k-1);0;0;w2(k-1)];
z(:,:,k,i)=H*x(:,:,k,i)+[v1(k); v2(k)];
end


Q = [0 0 0 0 0 0;
0 0 0 0 0 0;
0 0 sigma1p 0 0 0;
0 0 0 0 0 0;
0 0 0 0 0 0;
0 0 0 0 0 sigma2p]; % matricea de covarianta a
zgomotului de proces

xhatminus(:,:,:,i)=zeros(6,1,sz); % estimatul a priori a
lui x
Pminus(:,:,:,i)=zeros(6,6,sz); % estimatul erorii a
priori
TEFO Lucrarea 6 Filtrul Kalman

15
K(:,:,1,i)=[0.95 0; 0.1 0; 2e-4 0; 0 0.85; 0 0.1; 0 1e-5];
% castigul sau factorul de amestec initial

R = [sigmarp 0;
0 sigmatp]; % matricea de covarianta a zgomotului de
masurare

% ghicirea initiala
xhat(:,:,1,i) = zeros(6,1); % estimatul a posteriori a lui
x
P(:,:,1,i) = [sigmarp sigmarp/T(i) 0 0 0 0;
sigmarp/T(i) 2*sigmarp/(T(i)^2)+sigma1p 0 0 0
0;
0 0 sigma1p 0 0 0;
0 0 0 sigmatp sigmatp/T(i) 0;
0 0 0 sigmatp/T(i) 2*sigmatp/(T(i)^2)+sigma2p
0;
0 0 0 0 0 sigma2p]; % estimatul erorii
a posteriori

for k=2:n_iter
% actualizare in timp
xhatminus(:,:,k,i) = A*xhat(:,:,k-1,i);
Pminus(:,:,k,i) = A*P(:,:,k-1,i)*A'+Q;

% actualizare a masurarii
K(:,:,k,i) = Pminus(:,:,k,i)*H'*inv(
H*Pminus(:,:,k,i)*H'+R );
xhat(:,:,k,i) =
xhatminus(:,:,k,i)+K(:,:,k,i)*(z(:,:,k,i)-
H*xhatminus(:,:,k,i));
P(:,:,k,i) = (eye(6)-K(:,:,k,i)*H)*Pminus(:,:,k,i);
end
end

valid_iter = [1:n_iter]-1;

figure(1);
for i=1:3
for k=1:n_iter
P_depl(i,k)=P(1,1,k,i);
end
end
plot(valid_iter,P_depl(1,:),'--ko',valid_iter,P_depl(2,:),'-
ks',valid_iter,P_depl(3,:),':kv');
xlabel('Iteratie');
ylabel('Covarianta deplasarii');
legend('5s','10s','15s');
grid on;

figure(2);
TEFO Lucrarea 6 Filtrul Kalman

16
for i=1:3
for k=1:n_iter
P_vit_depl(i,k)=P(2,2,k,i);
end
end
plot(valid_iter,P_vit_depl(1,:),'--
ko',valid_iter,P_vit_depl(2,:),'-
ks',valid_iter,P_vit_depl(3,:),':kv');
xlabel('Iteratie');
ylabel('Covarianta vitezei de deplasare');
legend('5s','10s','15s');
grid on;


figure(3);
for i=1:3
for k=1:n_iter
P_unghi(i,k)=P(4,4,k,i);
end
end
plot(valid_iter,P_unghi(1,:),'--ko',valid_iter,P_unghi(2,:),'-
ks',valid_iter,P_unghi(3,:),':kv');
xlabel('Iteratie');
ylabel('Covarianta unghiului');
legend('5s','10s','15s');
grid on;

figure(4);
for i=1:3
for k=1:n_iter
P_vit_unghi(i,k)=P(5,5,k,i);
end
end
plot(valid_iter,P_vit_unghi(1,:),'--
ko',valid_iter,P_vit_unghi(2,:),'-
ks',valid_iter,P_vit_unghi(3,:),':kv');
xlabel('Iteratie');
ylabel('Covarianta vitezei unghiulare');
legend('5s','10s','15s');
grid on;

figure(5);
for i=1:3
for k=1:n_iter
P_zg_vit_depl(i,k)=P(3,3,k,i);
end
end
plot(valid_iter,P_zg_vit_depl(1,:),'--
ko',valid_iter,P_zg_vit_depl(2,:),'-
ks',valid_iter,P_zg_vit_depl(3,:),':kv');
xlabel('Iteratie');
TEFO Lucrarea 6 Filtrul Kalman

17
ylabel('Covarianta zgomotului vitezei de deplasare');
legend('5s','10s','15s',4);
grid on;

figure(6);
for i=1:3
for k=1:n_iter
P_zg_vit_unghi(i,k)=P(6,6,k,i);
end
end
plot(valid_iter,P_zg_vit_unghi(1,:),'--
ko',valid_iter,P_zg_vit_unghi(2,:),'-
ks',valid_iter,P_zg_vit_unghi(3,:),':kv');
xlabel('Iteratie');
ylabel('Covarianta zgomotului vitezei unghiulare');
legend('5s','10s','15s',4);
grid on;

% figurile pentru castigurile Kalman
figure(7);
for i=1:3
for k=1:n_iter
K_depl(i,k)=K(1,1,k,i);
end
end
plot(valid_iter,K_depl(1,:),'--ko',valid_iter,K_depl(2,:),'-
ks',valid_iter,K_depl(3,:),':kv');
xlabel('Iteratie');
ylabel('Castigul Kalman al deplasarii');
legend('5s','10s','15s');
grid on;

figure(8);
for i=1:3
for k=1:n_iter
K_vit_depl(i,k)=K(2,1,k,i);
end
end
plot(valid_iter,K_vit_depl(1,:),'--
ko',valid_iter,K_vit_depl(2,:),'-
ks',valid_iter,K_vit_depl(3,:),':kv');
xlabel('Iteratie');
ylabel('Castigul Kalman al vitezei de deplasare');
legend('5s','10s','15s');
grid on;

valid_iter = [1:n_iter-1];
figure(9);
for i=1:3
for k=1:n_iter-1
K_unghi(i,k)=K(4,2,k+1,i);
TEFO Lucrarea 6 Filtrul Kalman

18
end
end
plot(valid_iter,K_unghi(1,:),'--ko',valid_iter,K_unghi(2,:),'-
ks',valid_iter,K_unghi(3,:),':kv');
xlabel('Iteratie');
ylabel('Castigul Kalman al unghiului');
axis([1 15 0.3 1])
legend('5s','10s','15s');
grid on;

figure(10);
for i=1:3
for k=1:n_iter-1
K_vit_unghi(i,k)=K(5,2,k+1,i);
end
end
plot(valid_iter,K_vit_unghi(1,:),'--
ko',valid_iter,K_vit_unghi(2,:),'-
ks',valid_iter,K_vit_unghi(3,:),':kv');
xlabel('Iteratie');
ylabel('Castigul Kalman al vitezei unghiulare');
axis([1 15 0 0.12])
legend(' 5s','10s','15s');
grid on;

figure(11);
for i=1:3
for k=1:n_iter-1
K_zg_vit_depl(i,k)=K(3,1,k+1,i);
end
end
plot(valid_iter,K_zg_vit_depl(1,:),'--
ko',valid_iter,K_zg_vit_depl(2,:),'-
ks',valid_iter,K_zg_vit_depl(3,:),':kv');
xlabel('Iteratie');
ylabel('Castigul Kalman al zgomotului vitezei de deplasare');
axis([1 15 0 2.5e-3])
legend('5s','10s','15s',4);
grid on;

figure(12);
for i=1:3
for k=1:n_iter-1
K_zg_vit_unghi(i,k)=K(6,2,k+1,i);
end
end
plot(valid_iter,K_zg_vit_unghi(1,:),'--
ko',valid_iter,K_zg_vit_unghi(2,:),'-
ks',valid_iter,K_zg_vit_unghi(3,:),':kv');
xlabel('Iteratie');
ylabel('Castigul Kalman al zgomotului vitezei unghiulare');
TEFO Lucrarea 6 Filtrul Kalman

19
axis([1 15 0 3e-4])
legend('5s','10s','15s',4);
grid on;


5. Aplicaii propuse

1. O secven aleatoare n timp discret este dat de
1 k k k
x Ax w
+
= +
unde 0.5 A = ,
0
x are medie 0 i varian 1, iar
k
w este zgomot alb de medie 0 i varian 1.
Ecuaia de observaie este dat de
k k k
z x v = +
cu
k
v zgomot alb de medie 0 i varian 1. Termenii
0
x ,
k
w i
k
v sunt toi de tip gaussian.
S se realizeze un program care s dea secvena estimat

k x prin filtrare Kalman. Se

vor considera diferite valori pentru variana estimat a msurrii.
2. S se realizeze acelai program ca la aplicaia 1 pentru cazul n care 1 A = , zgomotul
k
w are variana 30, iar zgomotul
k
v are variana 20. Restul parametrilor, precum i ecuaiile
rmn aceleai.