Filtro De Kallman.docx

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. Además el filtro es ampliamente usado en campos como procesamiento de señales y econometría. Para este trabajo nos hemos referenciado por ejemplos encontrados en las paginas de youtube e internet.

DESARROLLO DEL PROBLEMA Sistema lineal en el espacio de estado Se entiende como espacio de estado todos los posibles estados de un sistema dinámico. Cada estado corresponde a un punto del espacio de estado.

donde: 𝑤(𝑡) es ruido blanco de valor promedio igual a cero y con 𝑄(𝑡) en el intervalo de tiempo descrito como t. 𝑣(𝑡) es ruido blanco de valor promedio igual a cero y con varianza 𝑅(𝑡) en el intervalo de tiempo descrito como t. El filtro de Kalman permite estimar el estado x(t+dt) a partir de las mediciones anteriores de 𝑢(𝑡) , 𝑧(𝑡) 𝑄(𝑡) , 𝑅(𝑡) y las estimaciones anteriores de 𝑥(𝑡) Algoritmo del Filtro discreto de Kalman

Caso de tiempo discreto: Se tiene un sistema representado en el espacio de estado:

donde: 𝑤𝑘 es ruido blanco de valor promedio igual a cero y con varianza 𝑄𝑘 en el instante k. 𝑣𝑘 es ruido blanco de valor promedio igual a cero y con varianza 𝑅𝑘 en el instante k. El filtro de Kalman permite estimar el estado 𝑥𝑘 a partir de las mediciones anteriores de 𝑢𝑘−𝑖 , 𝑧𝑘−𝑖 , 𝑄𝑘−𝑖 , 𝑅𝑘−𝑖 y las estimaciones anteriores de 𝑥𝑘−𝑖 Caso de tiempo continuo: Se tiene un sistema representado en el espacio de estado:

El Filtro de Kalman es un algoritmo recursivo en el que el estado 𝑥𝑘 es considerado una variable aleatoria Gaussiana. El filtro de Kalman suele describirse en dos pasos: Predicción y Corrección.

Predicción Estimación a priori

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 float r; // measurement noise covariance float x; // estimated value

proceso de la covariazna de ruido covariazna de ruido de medicion

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); 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");

// input array=

// 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 para la eliminación del ruido ya que elimina selectivamente las señales indeseadas y permite acceder directamente a la información que se encuentra en la señal. Esto lo logra basándose en descripciones probabilísticas y utilizando los modelos de espacio de estados, llegando así a la eliminación de perturbaciones que superen inclusive la magnitud de la señal de interés.

BIBLIOGRAFIA

• Con el filtro de Kalman se puede conocer los estimados con la actualización de cada muestra a través del tiempo, lo que permite saber el comportamiento especifico del sistema en cada muestra, es decir, notando cambios muestra tras muestra, y no como un promedio de estas como sucede con otra clase de métodos.

- C. Harvey, Forecasting, structural time series models and the Kalman filter, 1989.

• 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.

- J. G. Díaz, A. M. Mejía y F. Arteaga, Aplicación de los filtro de Kalman a sistemas de Control, 2001. - Haykin S, Cognition is the Key to the Next Generation of Radar Systems, 2009

Obtenido de file:///C:/Users/Lenovo/Downloads/Dialnet AnalisisYAplicacionDelFiltroDeKalmanAUna SenalConRu-4320424.pdf