EL FILTRO DE KALMAN INELMATIC

- 1 -
EL FILTRO DE KALMAN


Introduccin

En el siguiente documento se explicar un mtodo para estimar los estados de un
sistema estocstico. El mtodo fue descrito por Rudolf E. Kalman en 1958.

En un sistema determinstico trabajaramos de la siguiente forma: En primer lugar,
construiramos un modelo del sistema a partir de las leyes fsicas que definen las
dinmicas. En segundo lugar, estudiaramos su estructura y su respuesta. Por ltimo, y
si fuera necesario, se disearan compensadores para alterar las caractersticas del
sistema o bien reguladores. Para ello tendramos adems un amplio conjunto de
experiencias que han sido aplicadas con anterioridad sobre estos sistemas. En la realidad
ocurre que los sistemas determinsticos son aproximaciones de lo que realmente son,
principalmente por tres motivos:

No existe un modelo matemtico perfecto de un sistema real.
Existen perturbaciones que no se pueden modelar de una forma determinstica.
Los sensores no son perfectos.

Esto nos lleva a plantearnos una serie de preguntas:

Cmo desarrollar modelos de sistemas que tengan en cuenta las incertidumbres.
Cmo estimo de una forma ptima los datos que me interesan de un sistema mal
modelado y con datos alterados por el ruido.
Cmo controlo de una forma ptima sistemas estocsticos.

Qu es el Filtro de Kalman

Es un algoritmo de procesado de datos ptimo recursivo. ptimo porque minimiza un
criterio determinado y porque incorpora toda la informacin que se le suministra para
determinar el filtrado. Recursivo porque no precisa mantener los datos previos, lo que
facilita su implementacin en sistemas de procesado en tiempo real. Por ltimo,
algoritmo de procesado de datos, ya que es un filtro, pensado para sistemas discretos.

El objetivo del filtro de Kalman es estimar los estados de una manera ptima, de manera
que se minimiza el ndice del error cuadrtico medio.

EL FILTRO DE KALMAN INELMATIC

- 2 -
Un sistema lineal se puede representar as:



De manera que su evolucin es expresada en espacio de estados por:

x(k+1) = Ax(k) + Bu(k) + v(k)
y(k) = Cx(k) + w(k)


Siendo:
x(k) Estado
y(k) Salida u observacin del sistema.
w(k) proceso estocstico asociado a la medida
v(k) proceso estocsticas asociado al sistema
A, B, C matrices determinsticas que definen la dinmica del sistema

Se deben asumir las condiciones siguientes:

E[x(0)] = x
0

E[w(k)] = 0 k
E[v(k)] = 0 k
E[x(0),w(k)] = 0 k
E[x(0),v(k)] = 0 k
E[v(k),w(j)] = 0 k
E[w(k),w(k)] = R(k)
E[w(k),w(k)] = 0 k j
E[v(k),v(k)] = Q(k)
E[w(k),w(j)] = 0 k j
E[x(0),x(k)] = P
0
k

Las matrices de covarianza Q(k) y R(k) son diagonales y por tanto simtricas.

En el sistema real podremos observar el valor de y(k) de manera directa con los
sensores adecuados. Esta medida incorporar una serie de incertidumbres asociadas: la
incertidumbre del sensor y la del sistema. Por otro lado solo podremos acceder al valor
y(k). En caso de necesitar la evolucin completa del estado x(k) y/o de precisar el valor
de la observacin ajena a las variaciones provocadas a la incertidumbre, tendremos que
estimar de alguna manera indirecta sus valores.

EL FILTRO DE KALMAN INELMATIC

- 3 -
El filtro de Kalman propone un mtodo para obtener un estimador ptimo del estado. Si
suponemos que ) ( k x es la estimacin en el instante k del estado. El filtro de Kalman
buscar obtener ese valor de estimacin de manera que se minimice el error cuadrtico
medio. Definiendo el error como la diferencia entre el valor real del estado y la
estimacin:

) ( ) ( ) ( k x k x k e = . [1]

El objetivo por tanto ser minimizar

{ } ) ( ). ( ) ( n e n e E n P
T
= . [2]

A la matriz P(n) se la conoce como matriz de covarianza del error. Dependiendo del
valor que tome n en [2], tendremos distintas representaciones del filtro de Kalman:

Si n = k+1, el filtrado ser de prediccin.
Si n = k, el filtrado ser de alisado.


Filtro de Kalman con filtrado:

El objetivo consiste en determinar los valores de ) (k x

conocidas las medidas

contaminadas y(0),y(1),,y(k) para que P(k) sea mnima, o visto desde otro punto de
vista, el objetivo es determinar los valores de ) 1 ( + k x a partir de las medidas
contaminadas de la observacin ) 1 ( + k y para que la matriz ) 1 ( + k P sea mnima.

El filtrado de ) 1 ( + k x propuesta por Kalman y Bucy, se realizar a partir del estado
anterior y de un factor de correccin que ser funcin del error.

El algoritmo tiene dos pasos que son ejecutados de forma iterativa. Prediccin: Antes de
tener la medida y(k+1) y Correccin o actualizacin del estado.

En la siguiente figura se puede ver el proceso de clculo:


EL FILTRO DE KALMAN INELMATIC

- 4 -




Paso 1: Prediccin-Extrapolacin-Estimacin


Calculamos una prediccin del estado x(k+1), lo notaremos como x(k+1). El valor de
la prediccin es calculado a partir del valor ms actualizado del estado (posteriormente
veremos que es el estado corregido en la parte final del algoritmo)

) ( ) ( ) 1 ( ' k Bu k x A k x + = + [3]

Predecimos el valor de la matriz de covarianza del error previo a la medida P(k+1)
(recordamos que estamos haciendo una previsin del estado). Como se indic en [2], el
error estar definido por:

e(k+1) = x(k+1) x(k+1). [4]

Por tanto e(k+1) en esta etapa de prediccin vale:

) ( ) ( ) ( ) ( ) ( ) 1 ( ' k Bu k x A k v k Bu k Ax k e + + = +

[ ][ ] { }
T
k v k x k x A k v k x k x A E k P ) ( )) ( ) ( ( ) ( )) ( ) ( ( ) 1 ( ' + + = +

Operando:

[ ][ ] { } { }
T T
k v k v E k x k x A k x k x A E k P ) ( ) ( ) ( ) ( ( ) ( ) ( ( ) 1 ( ' + = +

{ }
T
k v k v E ) ( ) ( Se define como la matriz de covarianza asociada al proceso: Q(k)
Elapa de pred|cc|r

1 Pred|cc|r de| eslado
) ( ) ( ) 1 ( ' k Bu k x A k x + = +

2 Pred|cc|r de |a ralr|z de
covar|arza
Q A k AP k P
T
+ = + ) ( ) 1 ( '

Elapa de correcc|r

1 C|cu|o de| veclor de gararc|as
[ ]
1
) 1 ( ) 1 ( ' ) 1 ( ' ) 1 (

+ + + + = + k R C k CP k CP k K
T

2 Aclua||zac|r de| eslado

[ ] ) 1 ( ' ) 1 ( ) 1 ( ) 1 ( ' ) 1 ( + + + + + = + k x C k y k K k x k x


3 Aclua||zac|r de |a ralr|z de covar|arza
[ ] ) 1 ( ' ) 1 ( + = + k P KC I k P
Valores iniciales para ) ( k x y P(k)
EL FILTRO DE KALMAN INELMATIC

- 5 -
Queda por tanto:

Q A k AP k P
T
+ = + ) ( ) 1 ( ' (k) [5]



Paso 2: Innovacin-Actualizacin-Correccin.


Decamos antes que el valor del estado se va a calcular a travs del estado anterior y de
una correccin que es funcin del error. Esto es:

[ ] ) 1 ( ' ) 1 ( ) 1 ( ) 1 ( ' ) 1 ( + + + + + = + k x C k y k K k x k x [6]

Siendo el factor de correccin:

[ ] ) 1 ( ' ) 1 ( ) 1 ( + + + k x C k y k K
y:
y(k+1) es el ltimo valor observado.
x(k+1) es el valor ms actualizado disponible del estado (calculado en la fase de
prediccin.
Buscaremos el valor de K(k+1) para conseguir un valor ptimo de ) 1 ( + k x tal que la
matriz de covarianza del error sea mnima. Volviendo a definir el error a partir de [1]:

) 1 ( ) 1 ( ) 1 ( + + = + k x k x k e

Substituyendo:

[ ] ) 1 ( ' ) 1 ( ) 1 ( ' ) 1 ( ) 1 ( + + + + = + k Cx k y K k x k x k e

La observacin y(k+1) tiene el valor

) 1 ( ) 1 ( ) 1 ( + + + = + k w k Cx k y

Por tanto:

) 1 ( ' ) 1 ( ) 1 ( ) 1 ( ' ) 1 ( ) 1 ( + + + + + + = + k KCx k Kw k KCx k x k x k e

Agrupando:

[ ] ) 1 ( )) 1 ( ' ) 1 ( ( ) 1 ( + + + = + k Kw k x k x KC I k e

As pues, segn [2]:

{ [ ] ) 1 ( )) 1 ( ' ) 1 ( ( ) 1 ( + + + = + k Kw k x k x KC I E k P
[ ] }
T
k Kw k x k x KC I ) 1 ( )) 1 ( ' ) 1 ( ( + + +

EL FILTRO DE KALMAN INELMATIC

- 6 -
{ [ ] [ ] }+ + + + + = +
T
k x k x KC I k x k x KC I E k P )) 1 ( ' ) 1 ( ( )) 1 ( ' ) 1 ( ( ) 1 (
{ } k k w k kw E
T
) 1 ( ) 1 ( + +

{ }
T
k w k w E ) 1 ( ) 1 ( + + : Matriz de covarianza asociada a la medida: R(k+1)

[ ] { }[ ]
T
KC I k x k x k x k x E KC I k P + + + + = + ) 1 ( ' ) 1 ( ) 1 ( ' ) 1 ( ) 1 ( '
T
K k KR ) 1 ( + +

Anteriormente ya calculamos que:

{ }
T
k x k x k x k x E k P ) 1 ( ' ) 1 ( ) 1 ( ' ) 1 ( ) 1 ( ' + + + + = +

Por lo que :

[ ] [ ]
T T
KRK KC I k P KC I k P + + = + ) 1 ( ' ) 1 ( [7]

Si Llamamos P1 a P(k+1) y P(k+1) = P:

T T T T T
KRK K KCPC K PC KCP P P + + + + = 1

T T T
TrKRK K CPC TrK TrKCP TrP TrP + + + = ) ( 2 1 (Tr traza de la matriz)

Diferenciamos respecto a K para obtener el mnimo de la expresin e igualamos a cero

0 2 2 2 = + + =

KR TrCPC TrCP
K
TrP
T

[ ]
1

+ = R CPC CP K
T


Es decir:

[ ]
1
) 1 ( ) 1 ( ' ) 1 ( ' ) 1 (

+ + + + = + k R C k CP k CP k K
T
[8]

Se puede comprobar que es el mnimo a travs del Hessiano.

Substituyendo [8] en [7]:

[ ] ) 1 ( ' ) 1 ( + = + k P KC I K P [9]


Inicializacin del algoritmo:

Para iniciar el algoritmo es necesario conocer las siguientes condiciones de contorno:
Un valor inicial del estado: x(0). El valor de la matriz de covarianza del error, que para
P(0). Podemos asumir que P(0) = Q. Y los valores de las matrices de covarianza
asociadas al sistema y a la medida: Q y R.
EL FILTRO DE KALMAN INELMATIC

- 7 -
Resumiendo, el algoritmo tiene los siguientes pasos:

En la iteracin k:

1 Prediccin:

) ( ) ( ) 1 ( ' k Bu k x A k x + = +
Q A k AP k P
T
+ = + ) ( ) 1 ( '

2 Correccin

[ ]
1
) 1 ( ) 1 ( ' ) 1 ( ' ) 1 (

+ + + + = + k R C k CP k CP k K
T

observacin. y(k+1)
[ ] ) 1 ( ' ) 1 ( ) 1 ( ) 1 ( ' ) 1 ( + + + + + = + k x C k y k K k x k x
[ ] ) 1 ( ' ) 1 ( + = + k P KC I K P

Ntese que el concepto de filtrado es debido a que los valores del estado ptimo
) 1 ( + k x y de la observacin se producen en el mismo instante.

EL FILTRO DE KALMAN INELMATIC

- 8 -
Ejemplo 1. Filtrado de una constante


Supongamos que queremos estimar el valor de una constante que al ser medida tiene
una serie de desviaciones debidas a la incertidumbre del aparato de medida.

El sistema ser entonces:

x(k+1) = Ax(k) + Bu(k) + v(k)
y(k) = Cx(k) + w(k)


A=1
B=0
C=1

x(k+1) = x(k)+ v(k)
y(k) = x(k) + w(k)


Supongamos que la desviacin tpica de la medida es 5 . 0 =
w
. La covarianza (varianza
en este caso ser R
w
= = 25 . 0
2
. Y consideremos tambin que el sistema tiene una
desviacin muy pequea 001 . 0 =
v
. La varianza asociada al sistema es 6 1
2
= e v .
P(0) = 1e-6. En la siguiente grfica se puede observar el efecto del filtro de Kalman para
una constante de 30. x(0) = 25.


EL FILTRO DE KALMAN INELMATIC

- 9 -
Cdigo utilizado para generar la grfica:

X(1) = 25;
desv_oos = 0.5;
desv_eslado = 0.001;
var_oos = desv_oos desv_oos; R
var_eslado = desv_eslado desv_eslado; 0
P_despues(1) = var_oos; P(0)

lor |=1:100
1 Pred|cc|r
Pred|cc|r de| eslado
X_arles(|1)=X(|);
Pred|cc|r de |a ralr|z de covar|arza de| error
P_arles(|1)=P_despues(|)var_eslado;

2 Correcc|r
0ararc|a
0ararc|a(|1)=P_arles(|1)/(P_arles(|1)var_oos);
0oservac|r
Y(|1)=30 desv_oosrardr(1);
Esl|rac|r de| eslado
X(|1)=X_arles(|1)0ararc|a(|1)(Y(|1)-X_arles(|1));
Aclua||zac|r de |a ralr|z de covar|arza
P_despues(|1)=(1-0ararc|a(|))P_arles(|1);
erd



Ejemplo 1. Filtrado del estado para un sistema de segundo orden

Supongamos el siguiente sistema formado por un circuito RLC serie.



Si analizamos el sistema en el dominio de Laplace, obtendremos que:

1
1
) (
) (
2
0
+ +
=
RCs LCs s E
s E
i


Pasando a espacio de estados, (nota: el sistema es continuo):



EL FILTRO DE KALMAN INELMATIC

- 10 -
[ ] w
x
x
y
v
v
s E
LC
x
x
L
R
LC
x
x
i
+

2
1
2
1
2
1
2
1
0 1
0
0
) (
1
0
1
1 0

Siendo:
x
1
= E
0
x
2
=
dt
dE
0

Tomemos los valores

L = 1 H.
C = 1000 F
R = 30

As pues:

=
30 1000
1 0
A ;

=
1000
0
B ; [ ] 0 1 = C

Si presumimos un tiempo de muestreo de T = 0.01 s.

=
7001 . 0 4963 . 8
0085 . 0 9550 . 0
) (k A ;

=
4963 . 8
0450 . 0
) (k B ; [ ] 0 1 ) ( = k C

Suponemos una desviacin del observador de 1 voltio
Suponemos una desviacin del sistema de 0.01 voltio y 0.01 voltio/segundo.

01 . 0
1
=
v
; 01 . 0
2
=
v
; 1 =
w


por lo que :
R = 1; y

=
0001 . 0 0
0 0001 . 0
Q

En la siguiente grfica se puede ver el resultado de la estimacin


EL FILTRO DE KALMAN INELMATIC

- 11 -




Cdigo utilizado para generar la grfica:

0alos
C = 1e-3; L = 1; R = 30;
Forraros e| s|slera corl|ruo
A = [0 1 ; -1/(LC) -R/L|; 8 = [0 ; 1/(LC)|; C = [1 0|; 0 = 0;
3Y3 = ss(A,8,C,0);
Trarslorraros de corl|ruo a d|screlo cor T = 0.01 s.
sysd = c2d(3Y3,0.01);
[Ad,8d,Cd,0d|= ssdala(sysd);
gereraros |a erlrada
lor |=1:10 u(|)=1; erd
lor |=11:80 u(|)=2; erd
s|ru|aros e| s|slera leor|co
[Y_,X_| = d|s|r(Ad,8d,Cd,0d,u);
[s1,s2| = s|ze(X_);
cord|c|ores de corlorro
desv_oosvdr = 1; |rcerl|durore de |a ooservac|or (1 v)
desv_eslado = 0.05; |rcerl|durore de |os eslados (0.01 v y 0.01 v/s)
0 = [desv_eslado*2 0;0 desv_eslado*2|; ralr|z de covar|arza
R = desv_oosvdr*2; ralr|z de covar|arza (var|arza)
P_despues = 0; ralr|z de covar|arza de| error para l=0
X= [0;0|; eslado |r|c|a|
ouc|e
lor |=1:s1
1 Pred|cc|r
Pred|cc|r de| eslado
X_arles = AdX 8d(u(|) desv_esladorardr(1));
Pred|cc|r de |a ralr|z de covar|arza de| error
P_arles = AdP_despuesAd'0;
2 Correcc|r
ooservac|or (|a loraros de |a s|ru|ac|or arler|or)
Y = Y_(|) desv_oosvdrrardr(1);
0ararc|a
0ararc|a = CdP_arles|rv(CdP_arlesCd' R);
Esl|rac|r de| eslado
X = X_arles 0ararc|a' (Y - CdX_arles);
guardaros |os resu|lados para p|olear|os posler|orrerle
x1(|1)=X(1); x2(|1)=X(2); y(|1)=Y;
Aclua||zac|r de |a ralr|z de covar|arza
P_despues =(eye(2)-Cd' 0ararc|a)P_arles;
Erd


EL FILTRO DE KALMAN INELMATIC

- 12 -

Filtro de Kalman con estimacin:


Partimos de un sistema lineal representado por las ecuaciones:

) ( ) ( ) ( ) 1 ( k v k Bu k Ax k x + + = + [10]
) ( ) ( ) ( k w k Cx k y + =

Como ya se indic con anterioridad, la solucin que propusieron Kalman y Bucy para el
predictor del estado, vendr determinada por el estado en el instante anterior y por un
factor de correccin que ser funcin del error entre la observacin y la estimacin.

[ ] ) ( ) ( ) ( ) ( ) ( ) 1 ( k y k y k K k Bu k x A k x + = + [11]
) ( ) ( k x C k y =

Asumidas todas las condiciones iniciales, anlogas al caso de filtrado, se trata aqu de
determinar la estimacin ) 1 ( + k x , conociendo las medidas contaminadas de ruido y(0),
y(1), y(2),..., y(k), para que la matriz P(k+1) de covarianza del error, en el instante k+1,
sea mnima.

El error en el instante k+1 se determinar por:

) 1 ( ) 1 ( ) 1 ( + + = + k x k x k e [12]

Substituyendo en [12] las expresiones [10] y [11]

[ ] ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 1 ( k x C k w k Cx k K k Bu k x A k v k Bu k Ax k e + + + + = +

[ ] [ ] ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 1 ( k v k w k K k x k x k CK k x k x A k e + = +

[ ] ) ( ) ( ) ( k x k x k e =

[ ] ) ( ) ( ) ( ) ( ) ( ) 1 ( k v k w k K k e C k K A k e + = +

Llamando:

C k K A k A ) ( ) (

= [13]

tenemos:

) ( ) ( ) ( ) ( ) (

) 1 ( k v k w k K k e k A k e + = +




EL FILTRO DE KALMAN INELMATIC

- 13 -
As pues la matriz de covarianza del error tal y como se haba definido en [2]

{ }= + + = +
T
k e k e E k P ) 1 ( ) 1 ( ) 1 (

Substituyendo y operando:

{ [ ][ ] }= + +
T
k v k w k K k e k A k v k w k K k e k A E ) ( ) ( ) ( ) ( ) (

) ( ) ( ) ( ) ( ) (

{ +
T T T T T
v k e k A k w k K k e k A k A k e k e k A E ) ( ) (

) ( ) ( ) ( ) (

) (

) ( ) ( ) (

+
T T T T T
k v k w k K k e k A k w k K k K k w k w k K ) ( ) ( ) ( ) ( ) (

) ( ) ( ) ( ) ( ) ( ) (

}
T T T T T
k v k v k w k K k v k e k A k v ) ( ) ( ) ( ) ( ) ( ) ( ) (

) ( + +

y como:

{ } { } ) (

) ( ) (

) (

) ( ) ( ) (

) (

) ( ) ( ) (

k A k P k A k A k e k e E k A k A k e k e k A E
T T T T
= =

{ } { } 0 ) ( ) (

) ( ) ( ) ( ) ( ) ( ) (

= =
T T T T
k e k A k w k K E k w k K k e k A E

{ } { } 0 ) ( ) (

) ( ) ( ) ( ) (

= =
T T T
k e k A k v E k v k e k A E

{ } { }
T T T T T
k RK k K k K k w k w E k K k K k w k w k K E ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( = =

{ } { } 0 ) ( ) ( ) ( ) ( ) ( ) ( = =
T T T
k w k K k v E k v k w k K E

{ } Q k v k v E
T
= ) ( ) (

Queda:

Q k RK k K k A k P k A k P
T
+ + = + ) ( ) ( ) (

) ( ) (

) 1 (

Teniendo en cuenta el valor de ) (k A
T


[ ] [ ] Q k RK k K C k K A k P C k K A k P
T
+ + = + ) ( ) ( ) ( ) ( ) ( ) 1 ( [14]

Una vez que hemos calculado la expresin de la matriz de covarianza del error, tenemos
que encontrar el valor de K(k) que hace que esa matriz sea mnima:

[ ] [ ] Q k RK k K C k K A k P C k K A k P
T
+ + = + ) ( ) ( ) ( ) ( ) ( ) 1 (

+ + = +
T T T T T T
C k K k CP k K A k CP k K C k K k AP A k AP k P ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 1 (

Q k RK k K + ) ( ) (
EL FILTRO DE KALMAN INELMATIC

- 14 -

+ + = + ) ) ( ) ( ) ( ( ) ) ( ) ( ( ) ) ( ( ) 1 (
T T T T
C k K k CP k K Tr A k CP k K Tr A k AP Tr k TrP

) ( ) ( ) ( ( Q Tr k RK k K Tr +

Si diferenciamos respecto a K(k)

) ) ( ( 2 ) ) ( ) ( ( 2 ) ) ( ( 2
) (
) 1 (
R k K Tr C k CP k K Tr A k CP Tr
k K
k TrP
T T
+ + =

+


Igualando a cero para buscar el mnimo:

0 ) ) ( ( 2 ) ) ( ) ( ( 2 ) ) ( ( 2 = + + R k K Tr C k CP k K Tr A k CP Tr
T T


Al ser P(k) y R(k) simtricas:

[ ]
T T
A k CP R C k CP k K ) ( 2 ) ( ) ( = +

[ ]
1
) ( ) ( ) (

+ = R C k CP A k CP k K
T T
[15]

Substituyendo este valor en [14] obtenemos P(k+1)

[ ]
T
A k P C k K A Q k P ) ( ) ( ) 1 ( + = + [16]


Resumiendo, el proceso iterativo a seguir es:

1.- Clculo de la ganancia:

[ ]
1
) ( ) ( ) (

+ = R C k CP A k CP k K
T T


2.- Estimacin del estado:

[ ] ) ( ) ( ) ( ) ( ) ( ) 1 ( k x C k y k K k Bu k x A k x + = +

3.- Actualizacin de la matriz de covarianza del error

[ ]
T
A k P C k K A Q k P ) ( ) ( ) 1 ( + = +

Ntese que el concepto de estimacin es debido a que cuando tenemos la observacin
del sistema y(k) los valores obtenidos para el estado ptimo son los de ) 1 ( + k x .


EL FILTRO DE KALMAN INELMATIC

- 15 -
Si representamos el filtro en forma de bloques, obtenemos la siguiente representacin:


EL FILTRO DE KALMAN INELMATIC

- 16 -
Documentacin


[1] Rafael Molina Soriano. Bases Del Filtro de Kalman.
http://decsai.ugr.es/vip/doctorado/pvd/T7bn.pdf

[2] Tema XVII El filtro de Kalman
http://www.depeca.uah.es/wwwnueva/docencia/ ING-CA/ctr_avz/VVEE17.PDF

[3] Greg Welch, Gary Bishop. Introduction to the Kalman Filter.
Peter S. Haybeck. Stochastic models, estimation and control. Volume 1
Greg Welch, Gary Bishop. SCAAT. Incremental tracking with Incomplete
Information
http://knaan.com/Lev/SIGGRAPH2001_CoursePack_08.pdf

[4] Kalman Filter Derivation.
http://www.aticourses.com/kalman_filter.pdf