El filtro Kalman
43
1 1 El filtro Kalman Dr. P. Jorge Escamilla Ambrosio Centro de Investigación en Computación [email protected] [email protected]
Transcript of El filtro Kalman
1 1
El filtro Kalman
Dr P Jorge Escamilla Ambrosio
Centro de Investigacioacuten en Computacioacuten
pescamillacicipnmx
pjorgeeagmailcom
2
El filtro Kalman
Introduccioacuten
Filtro Kalman estaacutendar
Ejemplos
Filtro Kalman extendido
Ejemplos
SLAM con Filtro Kalman
Ejemplos
Arquitecturas de fusioacuten de sensores con el
Filtro Kalman
3
El Filtro Kalman
Es un estimador oacuteptimo del primer y segundo
momentos estadiacutesticos de un vector de estados
En el sentido de miacutenima varianza del error en la
estimacioacuten (error cuadraacutetico medio)
bull Si se satisfacen ciertas condiciones
Es un algoritmo recursivo de procesamiento de
informacioacuten
Utiliza de manera eficiente mediciones ruidosas
para estimar el vector de estados de un sistema
4
El filtro Kalman
Utiliza toda la informacioacuten disponible para
estimar el valor de las variables de intereacutes
Modelos dinaacutemicos del sistema o proceso en
cuestioacuten y del o los dispositivo de medicioacuten
La descripcioacuten estadiacutestica del ruido en el
sistema el error en la medicioacuten y la
incertidumbre en los modelos dinaacutemicos
Cualquier informacioacuten disponible sobre las
condiciones iniciales de las variables de intereacutes
5
El filtro Kalman Aplicacioacuten tiacutepica
Sistema
Dispositivo
de medicioacuten Filtro
Kalman
Sentildeal de
control
Fuentes de error del sistema
Fuentes de error de la medicioacuten
Estado del sistema (deseado pero desconocido)
Mediciones observadas
Estimacioacuten oacuteptima del estado del sistema
6
Problema general de
estimacioacuten secuencial
Considerar un sistema con un vector de
estados x que es n-dimensional
Determinar la distribucioacuten de probabilidad
del vector de estados en el instante de
tiempo k dado un valor inicial particular
del vector de estados x(0) y la
subsiguiente secuencia de mediciones y
sentildeales de control
7
Lo anterior puede representarse como una
funcioacuten de densidad de probabilidad conjunta
(joint probability density function)
donde z(0 k) y u(0 k) son el conjunto completo
de observaciones y sentildeales de control del
instante de tiempo 0 al instante de tiempo k
Problema general de
estimacioacuten secuencial
8
se busca una solucioacuten recursiva tal que si se tiene una
estimacioacuten de la distribucioacuten de probabilidad del vector de
estados en el instante de tiempo anterior k minus1
eacutesta se pueda utilizar junto con la uacuteltima entrada de control
u(k) y la uacuteltima observacioacuten z(k) para calcular la
distribucioacuten de probabilidad del vector de estados en el
instante de tiempo actual k
Problema general de
estimacioacuten secuencial
9
Considerando el vector de estados anterior x(k minus 1) y una entrada de control conocida u(k) un modelo del proceso predice la nueva distribucioacuten de probabilidad del vector de estados al instante de tiempo k
Entonces basado en todas las entradas de control y observaciones (mediciones) anteriores la distribucioacuten de probabilidad del vector de estados puede actualizarse de manera recursiva para tomar en consideracioacuten el efecto de las nuevas entradas de control por medio de la marginalizacioacuten de la dependencia en los estados precedentes
Problema general de
estimacioacuten secuencial
10
Por otra parte un modelo de medicioacuten utiliza la distribucioacuten de probabilidad del vector de estados actual x(k) para definir la distribucioacuten de probabilidad del conjunto de observaciones generadas en el instante de tiempo actual
y permite que la distribucioacuten de probabilidad del vector de estados sea recursivamente actualizada utilizando el Teorema de Bayes tomando en cuenta el efecto de las observaciones nuevas
Problema general de
estimacioacuten secuencial
11
Esta secuencia recursiva de marginalizacioacuten y acondicionamiento de la distribucioacuten de probabilidad del vector de estados es el proceso fundamental seguido por un filtro de estimacioacuten secuencial y es independiente del tipo de distribucioacuten de probabilidad
Si se asume que el vector de estados x tiene una distribucioacuten de probabilidad Gaussiana entonces sus estadiacutesticas se pueden representar utilizando su promedio x y covarianza Pxx
Esto conlleva a la utilizacioacuten del filtro Kalman como un estimador secuencial para el problema bajo consideracioacuten
Problema general de
estimacioacuten secuencial
12
Problema general de
estimacioacuten secuencial
13
El filtro Kalman
14
El filtro Kalman
15
El Filtro Kalman
Problema
Estimar el estado x n del sistema
Cuyo modelo de medicioacuten z m es
kkkkkk wuBxAx 1
kkkk vxHz
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
2
El filtro Kalman
Introduccioacuten
Filtro Kalman estaacutendar
Ejemplos
Filtro Kalman extendido
Ejemplos
SLAM con Filtro Kalman
Ejemplos
Arquitecturas de fusioacuten de sensores con el
Filtro Kalman
3
El Filtro Kalman
Es un estimador oacuteptimo del primer y segundo
momentos estadiacutesticos de un vector de estados
En el sentido de miacutenima varianza del error en la
estimacioacuten (error cuadraacutetico medio)
bull Si se satisfacen ciertas condiciones
Es un algoritmo recursivo de procesamiento de
informacioacuten
Utiliza de manera eficiente mediciones ruidosas
para estimar el vector de estados de un sistema
4
El filtro Kalman
Utiliza toda la informacioacuten disponible para
estimar el valor de las variables de intereacutes
Modelos dinaacutemicos del sistema o proceso en
cuestioacuten y del o los dispositivo de medicioacuten
La descripcioacuten estadiacutestica del ruido en el
sistema el error en la medicioacuten y la
incertidumbre en los modelos dinaacutemicos
Cualquier informacioacuten disponible sobre las
condiciones iniciales de las variables de intereacutes
5
El filtro Kalman Aplicacioacuten tiacutepica
Sistema
Dispositivo
de medicioacuten Filtro
Kalman
Sentildeal de
control
Fuentes de error del sistema
Fuentes de error de la medicioacuten
Estado del sistema (deseado pero desconocido)
Mediciones observadas
Estimacioacuten oacuteptima del estado del sistema
6
Problema general de
estimacioacuten secuencial
Considerar un sistema con un vector de
estados x que es n-dimensional
Determinar la distribucioacuten de probabilidad
del vector de estados en el instante de
tiempo k dado un valor inicial particular
del vector de estados x(0) y la
subsiguiente secuencia de mediciones y
sentildeales de control
7
Lo anterior puede representarse como una
funcioacuten de densidad de probabilidad conjunta
(joint probability density function)
donde z(0 k) y u(0 k) son el conjunto completo
de observaciones y sentildeales de control del
instante de tiempo 0 al instante de tiempo k
Problema general de
estimacioacuten secuencial
8
se busca una solucioacuten recursiva tal que si se tiene una
estimacioacuten de la distribucioacuten de probabilidad del vector de
estados en el instante de tiempo anterior k minus1
eacutesta se pueda utilizar junto con la uacuteltima entrada de control
u(k) y la uacuteltima observacioacuten z(k) para calcular la
distribucioacuten de probabilidad del vector de estados en el
instante de tiempo actual k
Problema general de
estimacioacuten secuencial
9
Considerando el vector de estados anterior x(k minus 1) y una entrada de control conocida u(k) un modelo del proceso predice la nueva distribucioacuten de probabilidad del vector de estados al instante de tiempo k
Entonces basado en todas las entradas de control y observaciones (mediciones) anteriores la distribucioacuten de probabilidad del vector de estados puede actualizarse de manera recursiva para tomar en consideracioacuten el efecto de las nuevas entradas de control por medio de la marginalizacioacuten de la dependencia en los estados precedentes
Problema general de
estimacioacuten secuencial
10
Por otra parte un modelo de medicioacuten utiliza la distribucioacuten de probabilidad del vector de estados actual x(k) para definir la distribucioacuten de probabilidad del conjunto de observaciones generadas en el instante de tiempo actual
y permite que la distribucioacuten de probabilidad del vector de estados sea recursivamente actualizada utilizando el Teorema de Bayes tomando en cuenta el efecto de las observaciones nuevas
Problema general de
estimacioacuten secuencial
11
Esta secuencia recursiva de marginalizacioacuten y acondicionamiento de la distribucioacuten de probabilidad del vector de estados es el proceso fundamental seguido por un filtro de estimacioacuten secuencial y es independiente del tipo de distribucioacuten de probabilidad
Si se asume que el vector de estados x tiene una distribucioacuten de probabilidad Gaussiana entonces sus estadiacutesticas se pueden representar utilizando su promedio x y covarianza Pxx
Esto conlleva a la utilizacioacuten del filtro Kalman como un estimador secuencial para el problema bajo consideracioacuten
Problema general de
estimacioacuten secuencial
12
Problema general de
estimacioacuten secuencial
13
El filtro Kalman
14
El filtro Kalman
15
El Filtro Kalman
Problema
Estimar el estado x n del sistema
Cuyo modelo de medicioacuten z m es
kkkkkk wuBxAx 1
kkkk vxHz
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
3
El Filtro Kalman
Es un estimador oacuteptimo del primer y segundo
momentos estadiacutesticos de un vector de estados
En el sentido de miacutenima varianza del error en la
estimacioacuten (error cuadraacutetico medio)
bull Si se satisfacen ciertas condiciones
Es un algoritmo recursivo de procesamiento de
informacioacuten
Utiliza de manera eficiente mediciones ruidosas
para estimar el vector de estados de un sistema
4
El filtro Kalman
Utiliza toda la informacioacuten disponible para
estimar el valor de las variables de intereacutes
Modelos dinaacutemicos del sistema o proceso en
cuestioacuten y del o los dispositivo de medicioacuten
La descripcioacuten estadiacutestica del ruido en el
sistema el error en la medicioacuten y la
incertidumbre en los modelos dinaacutemicos
Cualquier informacioacuten disponible sobre las
condiciones iniciales de las variables de intereacutes
5
El filtro Kalman Aplicacioacuten tiacutepica
Sistema
Dispositivo
de medicioacuten Filtro
Kalman
Sentildeal de
control
Fuentes de error del sistema
Fuentes de error de la medicioacuten
Estado del sistema (deseado pero desconocido)
Mediciones observadas
Estimacioacuten oacuteptima del estado del sistema
6
Problema general de
estimacioacuten secuencial
Considerar un sistema con un vector de
estados x que es n-dimensional
Determinar la distribucioacuten de probabilidad
del vector de estados en el instante de
tiempo k dado un valor inicial particular
del vector de estados x(0) y la
subsiguiente secuencia de mediciones y
sentildeales de control
7
Lo anterior puede representarse como una
funcioacuten de densidad de probabilidad conjunta
(joint probability density function)
donde z(0 k) y u(0 k) son el conjunto completo
de observaciones y sentildeales de control del
instante de tiempo 0 al instante de tiempo k
Problema general de
estimacioacuten secuencial
8
se busca una solucioacuten recursiva tal que si se tiene una
estimacioacuten de la distribucioacuten de probabilidad del vector de
estados en el instante de tiempo anterior k minus1
eacutesta se pueda utilizar junto con la uacuteltima entrada de control
u(k) y la uacuteltima observacioacuten z(k) para calcular la
distribucioacuten de probabilidad del vector de estados en el
instante de tiempo actual k
Problema general de
estimacioacuten secuencial
9
Considerando el vector de estados anterior x(k minus 1) y una entrada de control conocida u(k) un modelo del proceso predice la nueva distribucioacuten de probabilidad del vector de estados al instante de tiempo k
Entonces basado en todas las entradas de control y observaciones (mediciones) anteriores la distribucioacuten de probabilidad del vector de estados puede actualizarse de manera recursiva para tomar en consideracioacuten el efecto de las nuevas entradas de control por medio de la marginalizacioacuten de la dependencia en los estados precedentes
Problema general de
estimacioacuten secuencial
10
Por otra parte un modelo de medicioacuten utiliza la distribucioacuten de probabilidad del vector de estados actual x(k) para definir la distribucioacuten de probabilidad del conjunto de observaciones generadas en el instante de tiempo actual
y permite que la distribucioacuten de probabilidad del vector de estados sea recursivamente actualizada utilizando el Teorema de Bayes tomando en cuenta el efecto de las observaciones nuevas
Problema general de
estimacioacuten secuencial
11
Esta secuencia recursiva de marginalizacioacuten y acondicionamiento de la distribucioacuten de probabilidad del vector de estados es el proceso fundamental seguido por un filtro de estimacioacuten secuencial y es independiente del tipo de distribucioacuten de probabilidad
Si se asume que el vector de estados x tiene una distribucioacuten de probabilidad Gaussiana entonces sus estadiacutesticas se pueden representar utilizando su promedio x y covarianza Pxx
Esto conlleva a la utilizacioacuten del filtro Kalman como un estimador secuencial para el problema bajo consideracioacuten
Problema general de
estimacioacuten secuencial
12
Problema general de
estimacioacuten secuencial
13
El filtro Kalman
14
El filtro Kalman
15
El Filtro Kalman
Problema
Estimar el estado x n del sistema
Cuyo modelo de medicioacuten z m es
kkkkkk wuBxAx 1
kkkk vxHz
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
4
El filtro Kalman
Utiliza toda la informacioacuten disponible para
estimar el valor de las variables de intereacutes
Modelos dinaacutemicos del sistema o proceso en
cuestioacuten y del o los dispositivo de medicioacuten
La descripcioacuten estadiacutestica del ruido en el
sistema el error en la medicioacuten y la
incertidumbre en los modelos dinaacutemicos
Cualquier informacioacuten disponible sobre las
condiciones iniciales de las variables de intereacutes
5
El filtro Kalman Aplicacioacuten tiacutepica
Sistema
Dispositivo
de medicioacuten Filtro
Kalman
Sentildeal de
control
Fuentes de error del sistema
Fuentes de error de la medicioacuten
Estado del sistema (deseado pero desconocido)
Mediciones observadas
Estimacioacuten oacuteptima del estado del sistema
6
Problema general de
estimacioacuten secuencial
Considerar un sistema con un vector de
estados x que es n-dimensional
Determinar la distribucioacuten de probabilidad
del vector de estados en el instante de
tiempo k dado un valor inicial particular
del vector de estados x(0) y la
subsiguiente secuencia de mediciones y
sentildeales de control
7
Lo anterior puede representarse como una
funcioacuten de densidad de probabilidad conjunta
(joint probability density function)
donde z(0 k) y u(0 k) son el conjunto completo
de observaciones y sentildeales de control del
instante de tiempo 0 al instante de tiempo k
Problema general de
estimacioacuten secuencial
8
se busca una solucioacuten recursiva tal que si se tiene una
estimacioacuten de la distribucioacuten de probabilidad del vector de
estados en el instante de tiempo anterior k minus1
eacutesta se pueda utilizar junto con la uacuteltima entrada de control
u(k) y la uacuteltima observacioacuten z(k) para calcular la
distribucioacuten de probabilidad del vector de estados en el
instante de tiempo actual k
Problema general de
estimacioacuten secuencial
9
Considerando el vector de estados anterior x(k minus 1) y una entrada de control conocida u(k) un modelo del proceso predice la nueva distribucioacuten de probabilidad del vector de estados al instante de tiempo k
Entonces basado en todas las entradas de control y observaciones (mediciones) anteriores la distribucioacuten de probabilidad del vector de estados puede actualizarse de manera recursiva para tomar en consideracioacuten el efecto de las nuevas entradas de control por medio de la marginalizacioacuten de la dependencia en los estados precedentes
Problema general de
estimacioacuten secuencial
10
Por otra parte un modelo de medicioacuten utiliza la distribucioacuten de probabilidad del vector de estados actual x(k) para definir la distribucioacuten de probabilidad del conjunto de observaciones generadas en el instante de tiempo actual
y permite que la distribucioacuten de probabilidad del vector de estados sea recursivamente actualizada utilizando el Teorema de Bayes tomando en cuenta el efecto de las observaciones nuevas
Problema general de
estimacioacuten secuencial
11
Esta secuencia recursiva de marginalizacioacuten y acondicionamiento de la distribucioacuten de probabilidad del vector de estados es el proceso fundamental seguido por un filtro de estimacioacuten secuencial y es independiente del tipo de distribucioacuten de probabilidad
Si se asume que el vector de estados x tiene una distribucioacuten de probabilidad Gaussiana entonces sus estadiacutesticas se pueden representar utilizando su promedio x y covarianza Pxx
Esto conlleva a la utilizacioacuten del filtro Kalman como un estimador secuencial para el problema bajo consideracioacuten
Problema general de
estimacioacuten secuencial
12
Problema general de
estimacioacuten secuencial
13
El filtro Kalman
14
El filtro Kalman
15
El Filtro Kalman
Problema
Estimar el estado x n del sistema
Cuyo modelo de medicioacuten z m es
kkkkkk wuBxAx 1
kkkk vxHz
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
5
El filtro Kalman Aplicacioacuten tiacutepica
Sistema
Dispositivo
de medicioacuten Filtro
Kalman
Sentildeal de
control
Fuentes de error del sistema
Fuentes de error de la medicioacuten
Estado del sistema (deseado pero desconocido)
Mediciones observadas
Estimacioacuten oacuteptima del estado del sistema
6
Problema general de
estimacioacuten secuencial
Considerar un sistema con un vector de
estados x que es n-dimensional
Determinar la distribucioacuten de probabilidad
del vector de estados en el instante de
tiempo k dado un valor inicial particular
del vector de estados x(0) y la
subsiguiente secuencia de mediciones y
sentildeales de control
7
Lo anterior puede representarse como una
funcioacuten de densidad de probabilidad conjunta
(joint probability density function)
donde z(0 k) y u(0 k) son el conjunto completo
de observaciones y sentildeales de control del
instante de tiempo 0 al instante de tiempo k
Problema general de
estimacioacuten secuencial
8
se busca una solucioacuten recursiva tal que si se tiene una
estimacioacuten de la distribucioacuten de probabilidad del vector de
estados en el instante de tiempo anterior k minus1
eacutesta se pueda utilizar junto con la uacuteltima entrada de control
u(k) y la uacuteltima observacioacuten z(k) para calcular la
distribucioacuten de probabilidad del vector de estados en el
instante de tiempo actual k
Problema general de
estimacioacuten secuencial
9
Considerando el vector de estados anterior x(k minus 1) y una entrada de control conocida u(k) un modelo del proceso predice la nueva distribucioacuten de probabilidad del vector de estados al instante de tiempo k
Entonces basado en todas las entradas de control y observaciones (mediciones) anteriores la distribucioacuten de probabilidad del vector de estados puede actualizarse de manera recursiva para tomar en consideracioacuten el efecto de las nuevas entradas de control por medio de la marginalizacioacuten de la dependencia en los estados precedentes
Problema general de
estimacioacuten secuencial
10
Por otra parte un modelo de medicioacuten utiliza la distribucioacuten de probabilidad del vector de estados actual x(k) para definir la distribucioacuten de probabilidad del conjunto de observaciones generadas en el instante de tiempo actual
y permite que la distribucioacuten de probabilidad del vector de estados sea recursivamente actualizada utilizando el Teorema de Bayes tomando en cuenta el efecto de las observaciones nuevas
Problema general de
estimacioacuten secuencial
11
Esta secuencia recursiva de marginalizacioacuten y acondicionamiento de la distribucioacuten de probabilidad del vector de estados es el proceso fundamental seguido por un filtro de estimacioacuten secuencial y es independiente del tipo de distribucioacuten de probabilidad
Si se asume que el vector de estados x tiene una distribucioacuten de probabilidad Gaussiana entonces sus estadiacutesticas se pueden representar utilizando su promedio x y covarianza Pxx
Esto conlleva a la utilizacioacuten del filtro Kalman como un estimador secuencial para el problema bajo consideracioacuten
Problema general de
estimacioacuten secuencial
12
Problema general de
estimacioacuten secuencial
13
El filtro Kalman
14
El filtro Kalman
15
El Filtro Kalman
Problema
Estimar el estado x n del sistema
Cuyo modelo de medicioacuten z m es
kkkkkk wuBxAx 1
kkkk vxHz
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
6
Problema general de
estimacioacuten secuencial
Considerar un sistema con un vector de
estados x que es n-dimensional
Determinar la distribucioacuten de probabilidad
del vector de estados en el instante de
tiempo k dado un valor inicial particular
del vector de estados x(0) y la
subsiguiente secuencia de mediciones y
sentildeales de control
7
Lo anterior puede representarse como una
funcioacuten de densidad de probabilidad conjunta
(joint probability density function)
donde z(0 k) y u(0 k) son el conjunto completo
de observaciones y sentildeales de control del
instante de tiempo 0 al instante de tiempo k
Problema general de
estimacioacuten secuencial
8
se busca una solucioacuten recursiva tal que si se tiene una
estimacioacuten de la distribucioacuten de probabilidad del vector de
estados en el instante de tiempo anterior k minus1
eacutesta se pueda utilizar junto con la uacuteltima entrada de control
u(k) y la uacuteltima observacioacuten z(k) para calcular la
distribucioacuten de probabilidad del vector de estados en el
instante de tiempo actual k
Problema general de
estimacioacuten secuencial
9
Considerando el vector de estados anterior x(k minus 1) y una entrada de control conocida u(k) un modelo del proceso predice la nueva distribucioacuten de probabilidad del vector de estados al instante de tiempo k
Entonces basado en todas las entradas de control y observaciones (mediciones) anteriores la distribucioacuten de probabilidad del vector de estados puede actualizarse de manera recursiva para tomar en consideracioacuten el efecto de las nuevas entradas de control por medio de la marginalizacioacuten de la dependencia en los estados precedentes
Problema general de
estimacioacuten secuencial
10
Por otra parte un modelo de medicioacuten utiliza la distribucioacuten de probabilidad del vector de estados actual x(k) para definir la distribucioacuten de probabilidad del conjunto de observaciones generadas en el instante de tiempo actual
y permite que la distribucioacuten de probabilidad del vector de estados sea recursivamente actualizada utilizando el Teorema de Bayes tomando en cuenta el efecto de las observaciones nuevas
Problema general de
estimacioacuten secuencial
11
Esta secuencia recursiva de marginalizacioacuten y acondicionamiento de la distribucioacuten de probabilidad del vector de estados es el proceso fundamental seguido por un filtro de estimacioacuten secuencial y es independiente del tipo de distribucioacuten de probabilidad
Si se asume que el vector de estados x tiene una distribucioacuten de probabilidad Gaussiana entonces sus estadiacutesticas se pueden representar utilizando su promedio x y covarianza Pxx
Esto conlleva a la utilizacioacuten del filtro Kalman como un estimador secuencial para el problema bajo consideracioacuten
Problema general de
estimacioacuten secuencial
12
Problema general de
estimacioacuten secuencial
13
El filtro Kalman
14
El filtro Kalman
15
El Filtro Kalman
Problema
Estimar el estado x n del sistema
Cuyo modelo de medicioacuten z m es
kkkkkk wuBxAx 1
kkkk vxHz
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
7
Lo anterior puede representarse como una
funcioacuten de densidad de probabilidad conjunta
(joint probability density function)
donde z(0 k) y u(0 k) son el conjunto completo
de observaciones y sentildeales de control del
instante de tiempo 0 al instante de tiempo k
Problema general de
estimacioacuten secuencial
8
se busca una solucioacuten recursiva tal que si se tiene una
estimacioacuten de la distribucioacuten de probabilidad del vector de
estados en el instante de tiempo anterior k minus1
eacutesta se pueda utilizar junto con la uacuteltima entrada de control
u(k) y la uacuteltima observacioacuten z(k) para calcular la
distribucioacuten de probabilidad del vector de estados en el
instante de tiempo actual k
Problema general de
estimacioacuten secuencial
9
Considerando el vector de estados anterior x(k minus 1) y una entrada de control conocida u(k) un modelo del proceso predice la nueva distribucioacuten de probabilidad del vector de estados al instante de tiempo k
Entonces basado en todas las entradas de control y observaciones (mediciones) anteriores la distribucioacuten de probabilidad del vector de estados puede actualizarse de manera recursiva para tomar en consideracioacuten el efecto de las nuevas entradas de control por medio de la marginalizacioacuten de la dependencia en los estados precedentes
Problema general de
estimacioacuten secuencial
10
Por otra parte un modelo de medicioacuten utiliza la distribucioacuten de probabilidad del vector de estados actual x(k) para definir la distribucioacuten de probabilidad del conjunto de observaciones generadas en el instante de tiempo actual
y permite que la distribucioacuten de probabilidad del vector de estados sea recursivamente actualizada utilizando el Teorema de Bayes tomando en cuenta el efecto de las observaciones nuevas
Problema general de
estimacioacuten secuencial
11
Esta secuencia recursiva de marginalizacioacuten y acondicionamiento de la distribucioacuten de probabilidad del vector de estados es el proceso fundamental seguido por un filtro de estimacioacuten secuencial y es independiente del tipo de distribucioacuten de probabilidad
Si se asume que el vector de estados x tiene una distribucioacuten de probabilidad Gaussiana entonces sus estadiacutesticas se pueden representar utilizando su promedio x y covarianza Pxx
Esto conlleva a la utilizacioacuten del filtro Kalman como un estimador secuencial para el problema bajo consideracioacuten
Problema general de
estimacioacuten secuencial
12
Problema general de
estimacioacuten secuencial
13
El filtro Kalman
14
El filtro Kalman
15
El Filtro Kalman
Problema
Estimar el estado x n del sistema
Cuyo modelo de medicioacuten z m es
kkkkkk wuBxAx 1
kkkk vxHz
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
8
se busca una solucioacuten recursiva tal que si se tiene una
estimacioacuten de la distribucioacuten de probabilidad del vector de
estados en el instante de tiempo anterior k minus1
eacutesta se pueda utilizar junto con la uacuteltima entrada de control
u(k) y la uacuteltima observacioacuten z(k) para calcular la
distribucioacuten de probabilidad del vector de estados en el
instante de tiempo actual k
Problema general de
estimacioacuten secuencial
9
Considerando el vector de estados anterior x(k minus 1) y una entrada de control conocida u(k) un modelo del proceso predice la nueva distribucioacuten de probabilidad del vector de estados al instante de tiempo k
Entonces basado en todas las entradas de control y observaciones (mediciones) anteriores la distribucioacuten de probabilidad del vector de estados puede actualizarse de manera recursiva para tomar en consideracioacuten el efecto de las nuevas entradas de control por medio de la marginalizacioacuten de la dependencia en los estados precedentes
Problema general de
estimacioacuten secuencial
10
Por otra parte un modelo de medicioacuten utiliza la distribucioacuten de probabilidad del vector de estados actual x(k) para definir la distribucioacuten de probabilidad del conjunto de observaciones generadas en el instante de tiempo actual
y permite que la distribucioacuten de probabilidad del vector de estados sea recursivamente actualizada utilizando el Teorema de Bayes tomando en cuenta el efecto de las observaciones nuevas
Problema general de
estimacioacuten secuencial
11
Esta secuencia recursiva de marginalizacioacuten y acondicionamiento de la distribucioacuten de probabilidad del vector de estados es el proceso fundamental seguido por un filtro de estimacioacuten secuencial y es independiente del tipo de distribucioacuten de probabilidad
Si se asume que el vector de estados x tiene una distribucioacuten de probabilidad Gaussiana entonces sus estadiacutesticas se pueden representar utilizando su promedio x y covarianza Pxx
Esto conlleva a la utilizacioacuten del filtro Kalman como un estimador secuencial para el problema bajo consideracioacuten
Problema general de
estimacioacuten secuencial
12
Problema general de
estimacioacuten secuencial
13
El filtro Kalman
14
El filtro Kalman
15
El Filtro Kalman
Problema
Estimar el estado x n del sistema
Cuyo modelo de medicioacuten z m es
kkkkkk wuBxAx 1
kkkk vxHz
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
9
Considerando el vector de estados anterior x(k minus 1) y una entrada de control conocida u(k) un modelo del proceso predice la nueva distribucioacuten de probabilidad del vector de estados al instante de tiempo k
Entonces basado en todas las entradas de control y observaciones (mediciones) anteriores la distribucioacuten de probabilidad del vector de estados puede actualizarse de manera recursiva para tomar en consideracioacuten el efecto de las nuevas entradas de control por medio de la marginalizacioacuten de la dependencia en los estados precedentes
Problema general de
estimacioacuten secuencial
10
Por otra parte un modelo de medicioacuten utiliza la distribucioacuten de probabilidad del vector de estados actual x(k) para definir la distribucioacuten de probabilidad del conjunto de observaciones generadas en el instante de tiempo actual
y permite que la distribucioacuten de probabilidad del vector de estados sea recursivamente actualizada utilizando el Teorema de Bayes tomando en cuenta el efecto de las observaciones nuevas
Problema general de
estimacioacuten secuencial
11
Esta secuencia recursiva de marginalizacioacuten y acondicionamiento de la distribucioacuten de probabilidad del vector de estados es el proceso fundamental seguido por un filtro de estimacioacuten secuencial y es independiente del tipo de distribucioacuten de probabilidad
Si se asume que el vector de estados x tiene una distribucioacuten de probabilidad Gaussiana entonces sus estadiacutesticas se pueden representar utilizando su promedio x y covarianza Pxx
Esto conlleva a la utilizacioacuten del filtro Kalman como un estimador secuencial para el problema bajo consideracioacuten
Problema general de
estimacioacuten secuencial
12
Problema general de
estimacioacuten secuencial
13
El filtro Kalman
14
El filtro Kalman
15
El Filtro Kalman
Problema
Estimar el estado x n del sistema
Cuyo modelo de medicioacuten z m es
kkkkkk wuBxAx 1
kkkk vxHz
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
10
Por otra parte un modelo de medicioacuten utiliza la distribucioacuten de probabilidad del vector de estados actual x(k) para definir la distribucioacuten de probabilidad del conjunto de observaciones generadas en el instante de tiempo actual
y permite que la distribucioacuten de probabilidad del vector de estados sea recursivamente actualizada utilizando el Teorema de Bayes tomando en cuenta el efecto de las observaciones nuevas
Problema general de
estimacioacuten secuencial
11
Esta secuencia recursiva de marginalizacioacuten y acondicionamiento de la distribucioacuten de probabilidad del vector de estados es el proceso fundamental seguido por un filtro de estimacioacuten secuencial y es independiente del tipo de distribucioacuten de probabilidad
Si se asume que el vector de estados x tiene una distribucioacuten de probabilidad Gaussiana entonces sus estadiacutesticas se pueden representar utilizando su promedio x y covarianza Pxx
Esto conlleva a la utilizacioacuten del filtro Kalman como un estimador secuencial para el problema bajo consideracioacuten
Problema general de
estimacioacuten secuencial
12
Problema general de
estimacioacuten secuencial
13
El filtro Kalman
14
El filtro Kalman
15
El Filtro Kalman
Problema
Estimar el estado x n del sistema
Cuyo modelo de medicioacuten z m es
kkkkkk wuBxAx 1
kkkk vxHz
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
11
Esta secuencia recursiva de marginalizacioacuten y acondicionamiento de la distribucioacuten de probabilidad del vector de estados es el proceso fundamental seguido por un filtro de estimacioacuten secuencial y es independiente del tipo de distribucioacuten de probabilidad
Si se asume que el vector de estados x tiene una distribucioacuten de probabilidad Gaussiana entonces sus estadiacutesticas se pueden representar utilizando su promedio x y covarianza Pxx
Esto conlleva a la utilizacioacuten del filtro Kalman como un estimador secuencial para el problema bajo consideracioacuten
Problema general de
estimacioacuten secuencial
12
Problema general de
estimacioacuten secuencial
13
El filtro Kalman
14
El filtro Kalman
15
El Filtro Kalman
Problema
Estimar el estado x n del sistema
Cuyo modelo de medicioacuten z m es
kkkkkk wuBxAx 1
kkkk vxHz
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
12
Problema general de
estimacioacuten secuencial
13
El filtro Kalman
14
El filtro Kalman
15
El Filtro Kalman
Problema
Estimar el estado x n del sistema
Cuyo modelo de medicioacuten z m es
kkkkkk wuBxAx 1
kkkk vxHz
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
13
El filtro Kalman
14
El filtro Kalman
15
El Filtro Kalman
Problema
Estimar el estado x n del sistema
Cuyo modelo de medicioacuten z m es
kkkkkk wuBxAx 1
kkkk vxHz
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
14
El filtro Kalman
15
El Filtro Kalman
Problema
Estimar el estado x n del sistema
Cuyo modelo de medicioacuten z m es
kkkkkk wuBxAx 1
kkkk vxHz
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
15
El Filtro Kalman
Problema
Estimar el estado x n del sistema
Cuyo modelo de medicioacuten z m es
kkkkkk wuBxAx 1
kkkk vxHz
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
16
El Filtro Kalman
wk es un vector del ruido en el proceso
vk es un vector del ruido en la medicioacuten
Se asume que wk y vk son secuencias de
ruido blanco (variables aleatorias) son
independientes y con distribucioacuten de
probabilidad Gaussiana
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
17
Ruido blanco (white noise )
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
18
El Filtro Kalman
Estadiacutesticas de los ruidos wk y vk
para toda k
para toda k
para toda k e i
para toda k e i
para toda k
0kwE
0kvE
ki
kiQwwE
kT
ik0
ki
kiRvvE
kT
ik0
0T
ikvwE
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
19
El Filtro Kalman
ˆˆ10 xx
10 zz
1][ k
T
kkk
T
kkk RHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
]ˆ[ˆˆ kkkkkk xHzKxx
Realizar correccioacuten
dada la medicioacuten kzkkkkk uBxAx
ˆˆ
1
k
T
kkkk QAPAP
1
Realizar prediccioacuten
kkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCIOacuteN CORRECCIOacuteN
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
20
El Filtro Kalman
Valor a priori de la covarianza del error en la
estimacioacuten
Valor a posteriori de la covarianza del error en
la estimacioacuten
Recordar que
])ˆ)(ˆ[( T
kkkkk xxxxEP
])ˆ)(ˆ[( T
kkkkk xxxxEP
]))([()( 2XEXEXVar
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
21
El Filtro Kalman como un filtro
pasa bajas
))(ˆ)(()(ˆ)(ˆ txHtzKtxAtx
)()(ˆ)()(ˆ tKztxAKHtx
)()(ˆ))(( sKzsxAKHSI
Considere la estimacioacuten del vector de estados en el
dominio del tiempo continuo
Aplicando la transformada de Laplace resulta
)()(
)(ˆ)(
AKHsI
K
sz
sxsH
AKHc
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
22
El Filtro Kalman como un filtro
pasa bajas
)ˆ(ˆˆ kkkk xHzKxx
1)( k
T
kkk
T
kkk RHPHHPK
1
0lim
kk
RHK
k
Efecto de Q y R en la ganancia de Kalman
k
T
kkk
T
kk
RHPH
HP
0lim0
kP
Kk
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
23
El Filtro Kalman Ejemplo
Seguimiento de un objeto en movimiento (target
tracking)
ldquoDiscrete white noise acceleration (DWNA) modelrdquo
kkk GwAxx 1
10
1 TA
T
TG
2
2
1
kkk vHxz
01H
2
23
34
2
2
12
1
4
1
][ w
T
w
T
kk
TT
TTGGGwGwEQ
2
vR
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
24
El Filtro Kalman Ejemplo 22 )10( mv 2
2
2 )50(s
mw
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
25
El Filtro Kalman Extendido
Que sucede si el sistema es no lineal
Que pasa si el modelo de medicioacuten es no
lineal
El filtro Kalman que linealiza alrededor
del valor promedio y correspondiente
covarianza se denomina Filtro Kalman
Extendido
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
26
El Filtro Kalman Extendido
Modelo dinaacutemico del proceso en cuestioacuten
Modelo de medicioacuten
f y h deben ser diferenciables
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
27
Linealizacioacuten interpretacioacuten grafica
El Filtro Kalman Extendido
x0
y0
)(xfy Modelo
linealizado
Error grande
al alejarse de la
regioacuten valida
Regioacuten valida
Error
pequentildeo
Funcioacuten
no lineal
)( 00 yx
)()(
)(
0 n
afax
n
n
n
Serie de Taylor
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
28
The random variable x undergoes a nonlinear transformation
Statistics of y from the statistics of x
Using Taylor series expansion about x
Linearisation
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
29
Linearisation
Assuming that x is small ie that the mean of the random variable is a good
approximation of its true value then the higher order terms in the Taylor series
expansion can be ignored to give
Calculating the expectation value
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
30
Linearisation
The key step in the EKF formulation is to linearise the
process and measurement functions using a Taylor series
expansion so that the mean and covariance of the state
can be propagated through the filter
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
31
EKF
Prediction
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
32
EKF
Update
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
33
El Filtro Kalman Extendido
ˆˆ10 xx
10 zz
1][ T
kkk
T
kkk
T
kkk VRVHPHHPK
Calcular la ganancia Kalman
0x0P
Condiciones Iniciales and
)]0ˆ([ˆˆ kkkkkk xhzKxx
Realizar correccioacuten
dada la medicioacuten kz)0ˆ(ˆ1 kkk uxfx
T
kkk
T
kkkk WQWAPAP
1
Realizar prediccioacuten
kkkk PHKIP ][
Calcular la covarianza del
error en la estimacioacuten
PREDICCION CORRECCION
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
34
El Filtro Kalman Extendido Ejemplo
Estimacioacuten de una sentildeal seno aleatoria (la velocidad
angular y amplitud pueden variar en el tiempo)
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
35
El Filtro Kalman Extendido Ejemplo
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
36
El Filtro Kalman Extendido Ejemplo
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
37
El Filtro Kalman Extendido Ejemplo
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
38
Kalman filter Consistency
The filter estimates the mean and covariance of the state and it is
expected that the estimates should closely match the true values at
each step
A filter is said to be consistent if it converges to the true value of the
state
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
39
Kalman filter Consistency
Bar-Shalom et al [2002] define the criteria for
consistency as follows
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
40
Kalman filter Consistency
The NEES [Bar-Shalom et al 2002] evaluates the
consistency of the filter by looking at the Mahalanobis
distance between the true and estimated state
Under the assumptions that x(k|k) is a zero-mean Gaussian
random variable and that the filter is consistent then (k) is
expected to have a chi-square distribution with n degrees of
freedom where n is the dimension of x(k|k)
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
41
Kalman filter Consistency
The hypothesis that (k) is chi-square distributed is
accepted if it lies inside a two-sided interval
where
are the acceptance region bounds and is the significance
level
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
42
Kalman filter Consistency
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
43
Bibliografiacutea
Chui CK and Chen G Kalman filtering
with real-time applications Springer
Third edition 1999
Brown R G and Hwang P Y C
Introduction to random signals and
applied Kalman filtering woth matlab
exercises and solutions John Wiley
ampSons Third edition 1997
