FILTRO DE KALLMAN

MARIO ALBERTO MARQUEZ SALAZAR

PILLCO MITMA RONNY

GRANADOS PACHECO OSCAR

SULLCA JAYME

BERAUN RIVERA MIGUEL GERARDINO

RESUMEN
El filtro de Kalman es un algoritmo desarrollado
por Rudolf E. Kalman en 1960 que sirve para
poder identificar el estado oculto (no medible) de
un sistema dinámico lineal, al igual que
el observador de Luenberger. Además este filtro
contiene multiples aplicaciones a la tecnología
como es el caso de guía, navegación y control de
vehículos, especialmente naves espaciales. donde:
Además el filtro es ampliamente usado en
𝑤(𝑡) es ruido blanco de valor promedio igual a
campos como procesamiento de señales y
econometría. Para este trabajo nos hemos cero y con 𝑄(𝑡) en el intervalo de tiempo descrito
referenciado por ejemplos encontrados en las como t.
paginas de youtube e internet. 𝑣(𝑡) es ruido blanco de valor promedio igual a
cero y con varianza 𝑅(𝑡) en el intervalo de tiempo
descrito como t.
DESARROLLO DEL PROBLEMA El filtro de Kalman permite estimar el
estado x(t+dt) a partir de las mediciones
Sistema lineal en el espacio de estado anteriores de 𝑢(𝑡) , 𝑧(𝑡) 𝑄(𝑡) , 𝑅(𝑡) y las
estimaciones anteriores de 𝑥(𝑡)
Se entiende como espacio de estado todos los
posibles estados de un sistema dinámico. Cada Algoritmo del Filtro discreto de Kalman
estado corresponde a un punto del espacio de
estado.
Caso de tiempo discreto:
Se tiene un sistema representado en el
espacio de estado:

donde:
𝑤𝑘 es ruido blanco de valor promedio igual
El Filtro de Kalman es un algoritmo recursivo en
a cero y con varianza 𝑄𝑘 en el instante k.
el que el estado 𝑥𝑘 es considerado una variable
𝑣𝑘 es ruido blanco de valor promedio igual a aleatoria Gaussiana. El filtro de Kalman suele
cero y con varianza 𝑅𝑘 en el instante k. describirse en dos
pasos: Predicción y Corrección.
El filtro de Kalman permite estimar el
estado 𝑥𝑘 a partir de las mediciones
anteriores de 𝑢𝑘−𝑖 , 𝑧𝑘−𝑖 , 𝑄𝑘−𝑖 , 𝑅𝑘−𝑖 y las
estimaciones anteriores de 𝑥𝑘−𝑖
Predicción
Caso de tiempo continuo:
Estimación a priori
Se tiene un sistema representado en el espacio
de estado:

Covarianza del error asociada a la estimación a
priori
Corrección
Actualización de la medición
Ganancia de Kalman
Estimación a posteriori
Covarianza del error asociada a la estimación a
posterior
donde:
Matriz de Transición de estados. Es la
matriz que relaciona
en la ausencia de funciones forzantes (funciones
que dependen únicamente del tiempo y ninguna
otra variable).
El estimado apriori del vector de
estados.
Covarianza del error asociada a la
estimación a priori.
Vector de mediciones al momento k.
La matriz que indica la relación entre
mediciones y el vector de estado al
momento k en el supuesto ideal de que
no hubiera ruido en las mediciones.
La matriz de covarianza del ruido de las
mediciones (depende de la resolución
de los sensores).
Extensibilidad
En el caso de que el sistema dinámico sea no
lineal, es posible usar una modificación del
algoritmo llamada "Filtro de Kalman Extendido",
el cual linealiza el sistema en torno
al 𝑥(𝑡) identificado realmente, para calcular la
ganancia y la dirección de corrección adecuada.
En este caso, en vez de haber matrices A, B y C,
hay dos funciones 𝑓(𝑥,𝑢,𝑤) y ℎ(𝑥,𝑣) que entregan la
transición de estado y la observación (la salida
contaminada) respectivamente. El modelo lineal
dinámico con observación no lineal y
no Gaussiano se estudia en este caso. Se
extiende el teorema de Masreliez (ver. C. Johan
Masreliez, 1975) como una aproximación de
filtrado no Gaussiano con ecuación de estado
lineal y ecuación de observaciones también
lineal, al caso en que la ecuación de
observaciones no lineal pueda aproximarse
mediante el desarrollo en serie de Taylor de
segundo orden.
Primeras aplicaciones
Kalman encontró una audiencia receptiva de su
filtro en el verano de 1960 en una visita de
Stanley F. Schmidt del Ames Research
Center de NASA en Mountain View (California).
Kalman describió su resultado y Schmidt
reconoció su potencial aplicativo - la estimación
de la trayectoria y el problema del control
del programa Apolo. Schmidt comenzó a trabajar
inmediatamente en lo que fue probablemente la
primera implementación completa del filtro de
Kalman. Entusiasmado sobre el éxito del mismo,
Schmidt impulsó usar el filtro en trabajos
similares. A comienzos de 1961, Schmidt
describió sus resultados a Richard H. Battin del
laboratorio de instrumentación del MIT (llamado
más tarde el Charles Stark Draper Laboratory).
Battin estuvo usando métodos de espacio de
estado para el diseño y la implementación de
sistemas de navegación astronáutica, y él hizo al
filtro de Kalman parte del sistema de guía del
Apollo, el cual fue diseñado y desarrollado en el
laboratorio de instrumentación. A mediados de la
década de 1960, influenciado por Schmidt, el
filtro de Kalman se hizo parte del sistema de
navegación del transporte aéreo C5A, siendo
diseñado por Lockheed Aircraft Company. El
filtro de Kalman resolvió el problema de la fusión
de datos asociado con la combinación de los
datos del radar con los datos del sensor inercial
al lograr una aproximación global de la trayectoria
de la aeronave. Desde entonces ha sido parte
integral de la estimación de trayectorias a bordo
de las aeronaves y del diseño de sistemas de
control.
Impacto del filtro de Kalman en la tecnología
Desde el punto de vista de los problemas que
involucran control y estimación, el Filtro de
Kalman ha sido considerado el gran logro en la
teoría de estimación del siglo XX. Muchos de los
logros desde su introducción no hubiesen sido
posibles sin este. Se puede decir que el filtro de
Kalman fue una de las tecnologías que permitió
la era espacial ya que la precisión y eficiencia en
la navegación de las naves espaciales a través
del sistema solar puede no haber sido hecha sin
este. El principal uso del filtro de Kalman ha sido
en los sistemas de control modernos, en
eseguimiento y navegación de todo tipo de
vehículos, y en el diseño predictivo de estimación
de los mismos.

NUESTRO FILTRO
#include<stdio.h>

#include<math.h>

typedef struct kalman_struct{

float q; // process noise covariance proceso de la covariazna de ruido

float r; // measurement noise covariance covariazna de ruido de medicion

float x; // estimated value valor estimado

float p; // estimation error covariance error de estimacion covarianza

float k; // adaptive kalman filter gain ganancia adaptativa del filtro de kallman

}kalman_state;

int Kalmanfilter_C(float* InputArray, float* OutputArray, kalman_state* kstate, int Length); // input array=
matriz de entrada

void reset(kalman_state* kinit);

void subtract(float* sub, float* in1, float* in2, int length);

void misc(float* result, float* diff, int length);

float mean(float* input, int length2);

float root(float input);

float squarer(float input);

float correlation(float* in, float* out, int length);

void convolve(float* Result, float* in1, float* in2, int length);

int main(){

float input[11] = {0.39, 0.50, 0.48, 0.29, 0.25, 0.32, 0.34, 0.48, 0.41, 0.45,0.80};

int len = 11;

float output[len];

kalman_state kstate;

reset(&kstate);

Kalmanfilter_C(input, output, &kstate, len);

printf("\n");
 // subtract

printf("subtraction:\n");

float temp[len];

subtract(temp, input, output, len);

int j;

for(j = 0; j < len; j++){

printf("%f\n", temp[j]);

// misc

printf("\n");

float miscresult[2] = {0, 0};

misc(miscresult, temp, len);

printf("mean: %f stdev: %f\n", miscresult[0], miscresult[1]);

// correlation

printf("correlation: %f\n", correlation(input, output, len));

// convolution

printf("\n");

float holder[len];

int i;

for(i = 0; i < len; i++){

holder[i] = 0;

convolve(holder, input, output, len);

for(i = 0; i < len + len -1 ; i++){

printf("convolution[%d]= %f\n", i, holder[i]);

return 0;

void reset(kalman_state* kinit){

 kinit->q = 0.1;

kinit->r = 0.1;

kinit->x = 0.39;

kinit->p = 0.1;

kinit->k = 0;

int Kalmanfilter_C(float* InputArray, float* OutputArray, kalman_state* kstate,int Length){

int checker = 0;

int i;

for(i = 0; i < Length; i++){

kstate->p = kstate->p + kstate->q;

kstate->k = kstate->p / (kstate->p + kstate->r);

kstate->x = kstate->x + kstate->k * (InputArray[i] - kstate->x);

kstate->p = (1 - kstate->k) * kstate->p;

OutputArray[i] = kstate->x;

printf("Measured position: %f Kalman position: %f\n", InputArray[i], kstate->x);

if(kstate->x != kstate->x){

checker++;

if(checker > 0){

printf("Error: NaN!\n");

return 1;

return 0;

void subtract(float* sub, float* in1, float* in2, int length){

int i;
 for(i = 0; i < length; i++){

sub[i] = in1[i] - in2[i];

void misc(float* result, float* diff, int length){

result[0] = mean(diff, length);

float out = 0;

int i;

for(i = 0; i < length; i++){

out = out + squarer(diff[i] - result[0]);

result[1] = root(out / length);

float mean(float* input, int length2){

float result = 0;

int i;

for(i = 0; i < length2; i++){

result = result + input[i];

return (result / length2);

float root(float input){

int prev, k = 0;

int kmax = 1000;

float s = 1;

for(k = 0; k < kmax; k++){

prev = s;

s = (s + input/s)/2;
 if(prev == s){

break;

return s;

float squarer(float input){

return input * input;

float correlation(float* in, float* out, int length){

float mean_in = mean(in, length);

float mean_out = mean(out, length);

float temp1 = 0;

float temp2 = 0;

float temp3 = 0;

int i;

for(i = 0; i < length; i++){

temp1 = temp1 + ((in[i] - mean_in) * (out[i] - mean_out));

temp2 = temp2 + squarer(in[i] - mean_in);

temp3 = temp3 + squarer(out[i] - mean_out);

return(temp1 / root(temp2 * temp3));

void convolve(float* Result, float* in1, float* in2, int length){

int n;

for (n = 0; n < (length + length - 1); n++){

int kmin, kmax, k;

 Result[n] = 0;

kmin = (n >= length - 1) ? n - (length - 1) : 0;

kmax = (n < length - 1) ? n : length - 1;

for (k = kmin; k <= kmax; k++){

Result[n] += in1[k] * in2[n - k];

CONCLUSIONES
• El filtro de Kalman es una aplicación óptima BIBLIOGRAFIA
para la eliminación del ruido ya que elimina
selectivamente las señales indeseadas y permite - J. G. Díaz, A. M. Mejía y F. Arteaga,
acceder directamente a la información que se Aplicación de los filtro de Kalman a sistemas
encuentra en la señal. Esto lo logra basándose
de Control, 2001.
en descripciones probabilísticas y utilizando los
modelos de espacio de estados, llegando así a la
- Haykin S, Cognition is the Key to the Next
eliminación de perturbaciones que superen
inclusive la magnitud de la señal de interés. Generation of Radar Systems, 2009
• Con el filtro de Kalman se puede conocer los - C. Harvey, Forecasting, structural time
estimados con la actualización de cada muestra series models and the Kalman filter, 1989.
a través del tiempo, lo que permite saber el
comportamiento especifico del sistema en cada
- Obtenido de
muestra, es decir, notando cambios muestra tras
muestra, y no como un promedio de estas como file:///C:/Users/Lenovo/Downloads/Dialnet
sucede con otra clase de métodos. -
AnalisisYAplicacionDelFiltroDeKalmanAUna
SenalConRu-4320424.pdf
• El algoritmo de Kalman es una excelente
herramienta matemática ya que consta de una
etapa de predicción seguida de una etapa de
corrección

aplicada al proceso, lo que permite no solo

estimar sino filtrar de manera óptima la señal.

• Como se puede observar en la figura 5 se

presenta un desfase de una muestra entre la
señal real y la señal estimada y filtrada, esto es
debido básicamente a que la computación de los
datos requiere un tiempo de muestreo para
arrojar los resultados.