
\chapter{\protect\normalsize Filtro de Kalman}

\section{\protect\normalsize Introducción}

El filtro de Kalman (KF) es un procedimiento óptimo cuando se
trata de predecir eventos futuros en sistemas que continuamente
están cambiando. El KF da un estado aproximado producido por
definiciones recursivas, las cuales describen la estimación del
estado actual como una función solo del estado anterior y de la
nueva observación.

Supongamos que tenemos una variable $X(t_{k})$ cuyo valor deseamos
estimar en cierto tiempo $t_{0}$, $t_{1}$, $t_{2}$, etc. También ,
supongamos que $X(t_{k})$ satisface una ecuación dinámica lineal
\[
X(t_{k})=AX(t_{k-1})+Bw_{k}
\]


esta ecuación dinámica representa el conocimiento a priori que
necesitamos acerca de la variable, por lo que $A$ es la
información a priori para predecir un nuevo estado y $w_{k}$ es
ruido blanco con media cero y varianza $1.$ \ Esto es $E\left[
Bw_{k}\right] =0$ y $E\left[ \left( Bw_{k}\right) ^{2}\right]
=BB^{T}.$

El valor de $w_{k}$ es independiente de la variable $X(t_{k})$ y
de su antecesor $w_{k-1}$, lo cual indica que las variable no
están correlacionadas, características principal, del ruido blanco
$E\left[ X(t_{k})w_{k}\right] =0$.

Un filtro de Kalman necesita de una estimación inicial para
arrancar. Es como el motor de un automóvil que necesita de un
motor de marcha y una vez que arranco no lo necesita más.

Tendremos una estimación inicial para $X\left( t_{k}\right) $ dada
por $\widehat{X}\left( t_{0}\right) $ y para cualquier instante de
tiempo $t_{k}$ la predeciremos como
\[
\widehat{X}\left( t_{k}\right) =E\left[ X\left( t_{k}\right) \right] =A%
\widehat{X}\left( t_{k-1}\right)
\]


De manera similar, para la varianza $P$ tendremos un valor inicial
dado $P\left( t_{0}\right) $ y en cualquier tiempo la predeciremos
utilizando la ecuación de Riccati

\begin{equation}
P(t_{k})=AP(t_{k-1})A^{T}+BB^{T}
\end{equation}

Asumamos que hacemos una observación a la que llamaremos $Y$ y
consideraremos que esta esta relacionada linealmente con la variable $X$%
\[
Y(t_{k})=MX(t_{k})+Rw_{k}
\]
el valor esperado de $Y(t_{k})$ es
\[
\widehat{Y}(t_{k})=M\widehat{X}(t_{k})
\]

Nuestra mejor estimación no será $\widehat{X}(t_{k})$ ya que esta
estimación esta calculada a partir de modelo a priori y no tiene
información alguna de la observación. Así que
$\widehat{X}^{+}\left( t_{k}\right) $ será una mejor estimación y
esta dada por la ecuación

\begin{eqnarray}
\widehat{X}^{+}\left( t_{k}\right) =\widehat{X}\left( t_{k}\right)
+K_{k}\left( Y\left( t_{k}\right) -\widehat{Y}(t_{k})\right)  \nonumber \\
\widehat{X}^{+}\left( t_{k}\right) =\widehat{X}\left( t_{k}\right)
+K_{k}\left( Y\left( t_{k}\right) -M\widehat{X}(t_{k})\right)
\label{mejor}
\end{eqnarray}

donde \bigskip $K_{k}$ es un número que llamaremos ganancia de
Kalman.

Note que $Y\left( t_{k}\right) -\widehat{Y}(t_{k})$, en la
ecuación \ref{mejor}\ es exactamente el error al estimar $Y\left(
t_{k}\right) $, el cual en parte se debe al ruido $w_{k}$ y en
parte a la predicción o estimación de la señal (modelo a priori),
por lo que poner un valor unitario de $K_{k}$ sería un error.
Entonces ¿Que valor debemos usar?. Antes de decidir calculemos la
varianza de $X\left( t_{k}\right) $ respecto
a nuestra nueva estimación $\widehat{X}^{+}\left( t_{k}\right) $%
\begin{eqnarray*}
P^{+}\left( t_{k}\right) =E\left[ \left( X\left( t_{k}\right) -\widehat{X}%
^{+}\left( t_{k}\right) \right) ^{2}\right] \\
=E\left[ \left( X\left( t_{k}\right) -\widehat{X}\left(
t_{k}\right)
-K_{k}\left( Y(t_{k})-M\widehat{X}(t_{k})\right) \right) ^{2}\right] \\
=E\left[ \left( X\left( t_{k}\right) -\widehat{X}\left(
t_{k}\right) -K_{k}\left( MX\left( t_{k}\right)
+Rw_{k}-M\widehat{X}(t_{k})\right)
\right) ^{2}\right] \\
=E\left[ \left( X\left( t_{k}\right) -\widehat{X}\left(
t_{k}\right) -K_{k}\left( MX\left( t_{k}\right)
+Rw_{k}-M\widehat{X}(t_{k})\right)
\right) ^{2}\right] \\
=E\left[ \left( \left( 1-MK_{k}\right) \left( X\left( t_{k}\right) -\widehat{%
X}\left( t_{k}\right) \right) +K_{k}Rw_{k}\right) ^{2}\right]
\end{eqnarray*}
los productos cruzados son cero ya que el ruido no esta
correlacionada con la señal. Tenemos la formula final para la
varianza dada por
\begin{eqnarray*}
P^{+}\left( t_{k}\right) =\left( 1-MK_{k}\right) E\left[ \left(
X\left( t_{k}\right) -\widehat{X}\left( t_{k}\right) \right)
^{2}\right] \left( 1-MK_{k}\right) ^{T}+\left( K_{k}R\right)
E\left[ w_{k}^{2}\right] \left(
K_{k}R\right) ^{T} \\
P^{+}\left( t_{k}\right) =\left( 1-MK_{k}\right) P\left(
t_{k}\right) \left( 1-MK_{k}\right) ^{T}+\left( K_{k}R\right)
\left( K_{k}R\right) ^{T}
\end{eqnarray*}


En la formula anterior podemos ver que la varianza del error $%
P^{+}\left( t_{k}\right) $, la cual queremos minimizar, es función
de la ganancia de Kalman. Así que calculemos una nueva ganancia
$K_{k}^{*}$ que
minimice $P^{+}\left( t_{k}\right) $%
\[
\frac{\partial P^{+}\left( t_{k}\right) }{\partial K_{k}}=2\left(
1-MK_{k}\right) P\left( t_{k}\right) \left( -M\right)
+2RR^{T}K_{k}=0
\]
despejando tenemos
\[
K_{k}^{*}=\left[ MP\left( t_{k}\right) M^{T}+RR^{T}\right]
^{-1}MP\left( t_{k}\right)
\]


Los pasos que seguiremos para implementar el filtro de Kalman son

Paso 1.

Daremos un modelo a priori de acuerdo con las características de
la señal. Este modelo puede ser un proceso auto regresivo de
primer orden dado por

\[
\widehat{X}\left( t_{k}\right) =A\widehat{X}\left( t_{k-1}\right)
\]

Paso 2.

Utilizamos la ecuación recursiva de Riccati para hacer una
estimación de la varianza del error en términos de la varianza
anterior

\[
P(t_{k})=AP(t_{k-1})A^{T}+BB^{T}
\]

Paso 3.

Calculamos el valor de la ganancia de Kalman para minimizar la
varianza en la estimación.

\[
K_{k}^{*}=\left[ MP\left( t_{k}\right) M^{T}+RR^{T}\right]
^{-1}MP\left( t_{k}\right)
\]

Paso 4.

Haremos una corrección en la estimación con el valor $K_{k}^{*}$%
\[
X^{*}\left( t_{k}\right) =\widehat{X}\left( t_{k}\right) +\left[
K_{k}^{*}\right] ^{T}\left( Y\left( t_{k}\right)
-M\widehat{X}(t_{k})\right)
\]

Paso 5.

Hacemos la estimación final de la varianza

\[
P^{*}\left( t_{k}\right) =\left( 1-M^{T}K_{k}^{*}\right) P\left(
t_{k}\right) \left( 1-M^{T}K_{k}^{*}\right) ^{T}+\left( \left[
K_{k}^{*}\right] ^{T}R\right) \left( \left[ K_{k}^{*}\right]
^{T}R\right) ^{T}
\]

Estos cinco pasos se repetirán mientras exista una señal de
entrada, considerando únicamente que nuestra nueva estimación de
la varianza es $P\left( t_{k}\right) =P^{*}\left( t_{k}\right) $.
Los dos primeros pasos corresponderán a la predicción de la señal
y los siguientes a realizar la asimilación de la información,
pesada con la ganancia de Kalman.

En la figura \ref{fig:kalman} podemos observar un diagrama de
bloque que describe el funcionamiento del filtro de Kalman.


\begin{figure}[h!]
\centering \centerline{\psfig{figure=kalman.eps,width=8.0cm}}
\caption{Diagrama de aplicación del Filtro de Kalman}
\label{fig:kalman}
\end{figure}


\subsection{\protect\normalsize Ejemplo 1. Filtrado de una señal
uni--dimensional}

Consideremos que deseamos filtrar una señal unidimensional dada
como $Y(t_{k})=20+0.1\times t_{k}$ y ruido aditivo de observación
el cual esta dado como $N(0,7)$.

Deseamos diseñar un filtro de Kalman que nos permita filtrar la
señal. Necesitamos de un modelo a priori, es evidente que
desconozco la formula para $Y(k)$ pero se que se trata de una
señal sin variaciones bruscas, así que el modelo que propondré
será
\begin{eqnarray*}
X(t_{k})=AX(t_{k-1})+Bw_{k} \\
Y(t_{k})=MX(t_{k})+Rw_{k}
\end{eqnarray*}
el valor esperado para esta es
\begin{eqnarray*}
\widehat{X}(t_{k})=A\widehat{X}(t_{k-1}) \\
\widehat{Y}(t_{k})=M\widehat{X}(t_{k})
\end{eqnarray*}
con $A=1$, $M=1,$ $B^{2}=100$ y $R^{2}=1000$ y condiciones iniciales $%
\widehat{X}(t_{0})=30$ y$\ P(t_{0})=500$.

Para ver la aplicación de los pasos del filtro de Kalman,
calcularé la primer iteración

Paso I.

\[
\widehat{X}\left( t_{1}\right) =A\widehat{X}(t_{0})=1*30=30
\]


Paso II.
\[
P(t_{1})=A^{2}P(t_{0})+B^{2}=1*500+100=600
\]


Paso III.
\[
K_{1}^{*}=\frac{MP\left( t_{1}\right) }{M^{2}P\left( t_{1}\right) +R^{2}}=%
\frac{1*600}{1*600+1000}=0.375
\]

Paso IV.
\begin{eqnarray*}
X^{*}\left( t_{1}\right) =\widehat{X}\left( t_{1}\right)
+K_{1}^{*}\left(
Y\left( t_{1}\right) -M\widehat{X}(t_{1})\right) \\
=30+0375(20.931141-1*30) \\
=26.599177875
\end{eqnarray*}

Paso V
\begin{eqnarray*}
P^{*}\left( t_{1}\right) =\left( 1-MK_{1}^{*}\right) ^{2}P\left(
t_{1}\right) +\left( K_{1}^{*}R\right) ^{2} \\
=(1-1*0.375)^{2}600+(0.375)^{2}*1000 \\
=375
\end{eqnarray*}

En la figura \ref{fig:filtro} podemos ver el resultado del
filtrado de la señal $Y(k)$. En esta gráfica se muestra la señal
$Y(k)$ con ruido, la señal sin el ruido y la señal $X(k)$
filtrada. Es importante hacer notar que si consideramos un valor
de varianza $B^{2}$ pequeño, en el modelo
a priori tendríamos una señal plana dado que el modelo dinámico es $\widehat{%
X}\left( t_{k}\right) =\widehat{X}(t_{k-1}).$

\begin{figure}[h!]
{\normalsize \centering
\centerline{\psfig{figure=filtro.eps,width=8.0cm}} }
\caption{Ejemplo de filtrado utilizando KF} \label{fig:filtro}
\end{figure}

Finalmente es importante ver el comportamiento de la varianza de
la señal que estamos prediciendo, la cual converge a un valor
estable. En la figura \ref{fig:varianza}\ podemos ver su
evolución, para los datos de este ejemplo en particular.

\begin{figure}[h!]
\centering \centerline{\psfig{figure=varianza.eps,width=8.0cm}}
\caption{Comportamiento de la varianza} \label{fig:varianza}
\end{figure}


\subsection{\protect\normalsize Ejemplo 2. Seguimiento de un objeto con
aceleración constante}

Trataré de explicar la aplicación del filtro de Kalman para
realizar el Tracking en tiempo real de un objeto en movimiento. El
objeto puede ser, por ejemplo, un avión en pleno vuelo el cual se
mueve con aceleración constante.

Definamos un vector
\begin{equation}
\zeta (n)=[\zeta _{1}(n),\zeta _{2}(n),\zeta _{3}(n)]^{T}
\label{e21}
\end{equation}
que representa la posición del objeto en un espacio Cartesiano
tridimensional, en el tiempo $nT$, donde $T$ es el periodo de
muestreo.
Consideremos que $\stackrel{.}{\zeta }(n)$ es el vector de velocidad y $%
\stackrel{..}{\zeta }(n)$ es el vector de aceleración. Por
definición, el vector de velocidad y el vector de aceleración son
la primera y segunda derivada respectivamente de la trayectoria de
un objeto. Asumimos que la frecuencia de muestreo es lo
suficientemente grande para justificar la expresión para el vector
de posición del objeto a tiempo $(n+1)T$ como
\[
\zeta (n+1)=\zeta (n)+T\stackrel{.}{\zeta }(n)+\frac{1}{2}T^{2}\stackrel{..}{%
\zeta }(n)+\eta _{p}(n)
\]
donde $\eta _{p}(n)$ es un vector de error de truncamiento.
Similarmente podemos expresar el vector de velocidad como
\[
\stackrel{.}{\zeta }(n+1)=\stackrel{.}{\zeta }(n)+T\stackrel{..}{\zeta }%
(n)+\eta _{s}(n)
\]
donde $\eta _{s}(n)$ es un vector de error de truncamiento.
Similarmente podemos expresar el vector de aceleración como
\[
\stackrel{..}{\zeta }(n+1)=\stackrel{..}{\zeta }(n)+\eta _{a}(n)
\]
donde $\eta _{a}(n)$ es un vector de error de truncamiento

El vector de estado para el objeto lo definimos como
\[
X(n)=[\zeta _{1}(n),\stackrel{.}{\zeta _{1}}(n),\stackrel{..}{\zeta _{1}}%
(n),\zeta _{2}(n),\stackrel{.}{\zeta _{2}}(n),\stackrel{..}{\zeta _{2}}%
(n),\zeta _{3}(n),\stackrel{.}{\zeta _{3}}(n),\stackrel{..}{\zeta _{3}}%
(n)]^{T}
\]
el cual podemos escribir de manera simplificada como
\begin{equation}
X(n)=\left[
\begin{array}{c}
x_{1}(n) \\
x_{2}(n) \\
x_{3}(n)
\end{array}
\right]  \label{e22}
\end{equation}
donde
\[
x_{i}(n)=[\zeta _{i}(n),\stackrel{.}{\zeta _{i}}(n),\stackrel{..}{\zeta _{i}}%
(n)]^{T}
\]


De igual forma el vector de error de truncamiento lo definimos
como
\[
v(n)=[\eta _{p1}(n),\eta _{s1}(n),\eta _{a1}(n),\eta _{p2}(n),\eta
_{s2}(n),\eta _{a2}(n),\eta _{p3}(n),\eta _{s3}(n),\eta
_{a3}(n)]^{T}
\]
o de manera simple como


\begin{equation}
v_{i}(n)=[\eta _{pi}(n),\eta _{si}(n),\eta _{ai}(n)]^{T}
\label{e23}
\end{equation}


Podemos usar las ecuaciones \ref{e21}, \ref{e22}, y \ref{e23} para
expresar la ecuación de estado que modela el tracking del objeto
\[
\left[
\begin{array}{c}
x_{1}(n+1) \\
x_{2}(n+1) \\
x_{3}(n+1)
\end{array}
\right] =\left[
\begin{array}{ccc}
A & 0 & 0 \\
0 & A & 0 \\
0 & 0 & A
\end{array}
\right] \left[
\begin{array}{c}
x_{1}(n) \\
x_{2}(n) \\
x_{3}(n)
\end{array}
\right] +\left[
\begin{array}{c}
v_{1}(n) \\
v_{2}(n) \\
v_{3}(n)
\end{array}
\right]
\]
donde
\[
A=\left[
\begin{array}{ccc}
1 & T & \frac{1}{2}T^{2} \\
0 & 1 & T \\
0 & 0 & 1
\end{array}
\right]
\]

La matriz de transición de estado, es una matriz diagonal en
bloque, así que podemos simplificarla como
\[
x_{i}(n+1)=Ax_{i}(n)+v_{i}(n)
\]

El vector de observación $y(n)$ es un vector tridimensional que
podemos representar como
\[
Y(n)=\left[
\begin{array}{c}
\zeta _{1}(n) \\
\zeta _{2}(n) \\
\zeta _{3}(n)
\end{array}
\right] +w(n)
\]
donde $w(n)$ es el vector de error de medida. Podemos deducir que
la matriz
de medidas del modelo de tracking es una matriz dispersa de orden $3\times 9$%
\[
\left[
\begin{array}{ccccccccc}
1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0
\end{array}
\right]
\]

Podemos simplificar la expresión escribiendo
\[
Y(n)=\left[
\begin{array}{ccc}
M & 0 & 0 \\
0 & M & 0 \\
0 & 0 & M
\end{array}
\right] \left[
\begin{array}{c}
x_{1}(n) \\
x_{2}(n) \\
x_{3}(n)
\end{array}
\right] +w(n)
\]
donde $M=[1,0,0]$. Note que la matriz de medida es una matriz
diagonal en bloque, la cual podemos simplificar y escribir
\[
y_{i}(n)=Mx_{i}(n)+w_{i}(n)
\]

Finalmente nuestro modelo para hacer el tracking esta dado por:
\begin{eqnarray*}
x_{i}(n+1)=Ax_{i}(n)+v_{i}(n) \\
y_{i}(n)=Mx_{i}(n)+w_{i}(n)
\end{eqnarray*}
adicionalmente asumimos que
\begin{eqnarray*}
E\left[ v_{i}(n)v_{i}^{T}(k)\right] =Q(n)\delta (n-k) \\
E\left[ w_{i}(n)w_{i}^{T}(k)\right] =R(n)\delta (n-k)
\end{eqnarray*}

\subsection{\protect\normalsize Ejemplo 3. Determinación de parámetros.}

Consideremos un objeto en movimiento el cual queremos rastrear.
Lo único que sabemos de el es que se mueve en un espacio bidimensional en lí%
nea recta.

{\normalsize Comenzaremos por definir el vector de estados, con los pará%
metros que describen a una línea recta $X(k)=\left[
x_{1}(k),x_{2}(k)\right] ^{T}$ y representaremos las coordenadas
de un punto con un vector $\left[ d_{x},d_{y}\right] $. }

{\normalsize Nuestro modelo dinámico será
\[
X(k)=AX(k-1)+Bw_{k}\text{ \ \ con \ \ }A=\left[
\begin{array}{cc}
1 & 0 \\
0 & 1
\end{array}
\right]
\]
}

{\normalsize El vector de observaciones lo determinamos como
\[
d_{y}(k)=x_{1}(k)d_{x}(k)+x_{2}(k)
\]
en forma matricial }

{\normalsize
\[
d_{y}(k)=M(k)X(k)+Rw_{k}\text{ \ \ con\ \ \ }M=\left[
d_{x}(k),1\right]
\]
}

{\normalsize Nuestra predicción estará dada por
\begin{eqnarray*}
\widehat{X}(k)=\widehat{X}(k-1) \\
\widehat{d}_{y}(k)=M(k)\widehat{X}(k) \\
P(k)=AP(k-1)A^{T}+BB^{T}
\end{eqnarray*}
}

{\normalsize Con el propósito de probar estas ecuaciones, realice
una simulación donde
\[
d_{y}(k)=1.1d_{x}(k)+2.0
\]
los parámetros utilizados son
\[
\widehat{X}(0)=\left[ 0,0\right] ^{T},\text{ \ }B=\left[
\begin{array}{cc}
0.0001 & 0 \\
0 & 0.0001
\end{array}
\right] ,\text{ \ }P(0)=\left[
\begin{array}{cc}
0.1 & 0 \\
0 & 0.1
\end{array}
\right] \text{ \ y }R=16
\]
}

En las figura \ref{fig:rastreo01} vemos la línea recta que
deseamos modelar así como la línea que estamos prediciendo. En la
figura \ref {fig:rastreo02} podemos ver la convergencia de los
parámetros a medida que tenemos más muestras.

\begin{figure}[h!]
{\normalsize \centering
\centerline{\psfig{figure=rastreo01.eps,width=8.0cm} } }
\caption{Solución del cálculo de parámetros} \label{fig:rastreo01}
\end{figure}

\begin{figure}[h!]
{\normalsize \centering
\centerline{\psfig{figure=rastreo02.eps,width=8.0cm} } }
\caption{Parámetros calculados} \label{fig:rastreo02}
\end{figure}

En este ejemplo, se tiene una relación lineal entre los pará%
metros y la observación. De no existir esta linealidad tendremos
que hacer uso de la serie de Taylor para encontrar una expresión
lineal, lo que da lugar, al filtro de Kalman extendido.
