EL FILTRO DE KALMAN. Introducción. Qué es el Filtro de Kalman

Transcripción

1 L FILRO D LMN Introducción n l siguint documnto s xplicará un método para stimar los stados d un sistma stocástico. l método fu dscrito por Rudolf. alman n n un sistma dtrminístico trabajaríamos d la siguint forma: n primr lugar, construiríamos un modlo dl sistma a partir d las lys físicas qu dfinn las dinámicas. n sgundo lugar, studiaríamos su structura y su rspusta. Por último, y si fura ncsario, s disñarían compnsadors para altrar las caractrísticas dl sistma o bin rguladors. Para llo tndríamos admás un amplio conjunto d xprincias qu han sido aplicadas con antrioridad sobr stos sistmas. n la ralidad ocurr qu los sistmas dtrminísticos son aproximacions d lo qu ralmnt son, principalmnt por trs motios: No xist un modlo matmático prfcto d un sistma ral. xistn prturbacions qu no s pudn modlar d una forma dtrminística. Los snsors no son prfctos. sto nos lla a plantarnos una sri d prguntas: Cómo dsarrollar modlos d sistmas qu tngan n cunta las incrtidumbrs. Cómo stimo d una forma óptima los datos qu m intrsan d un sistma mal modlado y con datos altrados por l ruido. Cómo controlo d una forma óptima sistmas stocásticos. Qué s l Filtro d alman s un algoritmo d procsado d datos óptimo rcursio. Óptimo porqu minimiza un critrio dtrminado y porqu incorpora toda la información qu s l suministra para dtrminar l filtrado. Rcursio porqu no prcisa mantnr los datos prios, lo qu facilita su implmntación n sistmas d procsado n timpo ral. Por último, algoritmo d procsado d datos, ya qu s un filtro, pnsado para sistmas discrtos. l objtio dl filtro d alman s stimar los stados d una manra óptima, d manra qu s minimiza l índic dl rror cuadrático mdio

2 Un sistma linal s pud rprsntar así: D manra qu su olución s xprsada n spacio d stados por: x1 x Bu y Cx Sindo: x stado y Salida u obsración dl sistma. procso stocástico asociado a la mdida procso stocásticas asociado al sistma, B, C matrics dtrminísticas qu dfinn la dinámica dl sistma S dbn asumir las condicions siguints: [x0] x 0 [] 0 [] 0 [x0,] 0 [x0,] 0 [,j] 0 [,] R [,] 0 j [,] Q [,j] 0 j [x0,x] P 0 Las matrics d coarianza Q y R son diagonals y por tanto simétricas. n l sistma ral podrmos obsrar l alor d y d manra dircta con los snsors adcuados. sta mdida incorporará una sri d incrtidumbrs asociadas: la incrtidumbr dl snsor y la dl sistma. Por otro lado solo podrmos accdr al alor y. n caso d ncsitar la olución complta dl stado x y/o d prcisar l alor d la obsración ajna a las ariacions proocadas a la incrtidumbr, tndrmos qu stimar d alguna manra indircta sus alors

3 l filtro d alman propon un método para obtnr un stimador óptimo dl stado. Si suponmos qu x ˆ s la stimación n l instant dl stado. l filtro d alman buscará obtnr s alor d stimación d manra qu s minimic l rror cuadrático mdio. Dfinindo l rror como la difrncia ntr l alor ral dl stado y la stimación: x xˆ. [1] l objtio por tanto srá minimizar { n. } P n n. [2] la matriz Pn s la conoc como matriz d coarianza dl rror. Dpndindo dl alor qu tom n n [2], tndrmos distintas rprsntacions dl filtro d alman: Si n 1, l filtrado srá d prdicción. Si n, l filtrado srá d alisado. Filtro d alman con filtrado: l objtio consist n dtrminar los alors d x conocidas las mdidas contaminadas y0,y1,,y para qu P sa mínima, o isto dsd otro punto d ista, l objtio s dtrminar los alors d x ˆ 1 a partir d las mdidas contaminadas d la obsración y 1 para qu la matriz P 1 sa mínima. l filtrado d x ˆ 1 propusta por alman y Bucy, s ralizará a partir dl stado antrior y d un factor d corrcción qu srá función dl rror. l algoritmo tin dos pasos qu son jcutados d forma itratia. Prdicción: nts d tnr la mdida y1 y Corrcción o actualización dl stado. n la siguint figura s pud r l procso d cálculo: - 3 -

4 x 1 xˆ Bu 2 P 1 P Q [ CP 1 C R 1 ] 1 1 CP 1 x ˆ 1 x 1 1 y 1 C x 1 P 1 I C P 1 [ ] [ ] Valors inicials para x ˆ y P Paso 1: Prdicción-xtrapolación-stimación Calculamos una prdicción dl stado x1, lo notarmos como x 1. l alor d la prdicción s calculado a partir dl alor más actualizado dl stado postriormnt rmos qu s l stado corrgido n la part final dl algoritmo x 1 xˆ Bu [3] Prdcimos l alor d la matriz d coarianza dl rror prio a la mdida P 1 rcordamos qu stamos hacindo una prisión dl stado. Como s indicó n [2], l rror stará dfinido por: 1 x1 x 1. [4] Por tanto 1 n sta tapa d prdicción al: Oprando: 1 x Bu xˆ Bu {[ x xˆ ][ x xˆ ] } P 1 {[ x xˆ ][ x xˆ ] } { } P 1 { } S dfin como la matriz d coarianza asociada al procso: Q - 4 -

5 Quda por tanto: P 1 P Q [5] Paso 2: Innoación-ctualización-Corrcción. Dcíamos ants qu l alor dl stado s a a calcular a traés dl stado antrior y d una corrcción qu s función dl rror. sto s: [ y 1 C x 1 ] x ˆ 1 x 1 1 [6] Sindo l factor d corrcción: y: [ y 1 C x 1 ] 1 y1 s l último alor obsrado. x 1 s l alor más actualizado disponibl dl stado calculado n la fas d prdicción. Buscarmos l alor d 1 para consguir un alor óptimo d x ˆ 1 tal qu la matriz d coarianza dl rror sa mínima. Volindo a dfinir l rror a partir d [1]: 1 x 1 xˆ 1 Substituyndo: [ y 1 Cx 1 ] 1 x 1 x 1 La obsración y1 tin l alor Por tanto: grupando: y 1 Cx x 1 x 1 Cx 1 1 Cx 1 [ I C] x 1 x sí pus, sgún [2]: { [ I C] x 1 x 1 1 [ I C] x 1 x 1 1 } P 1-5 -

6 P 1 { 1 1 } { [ I C] x 1 x 1 [ I C] x 1 x 1 } { 1 1 } : Matriz d coarianza asociada a la mdida: R1 [ I C] { x 1 x 1 x 1 x 1 }[ I C] P 1 R 1 ntriormnt ya calculamos qu: Por lo qu : { x 1 x 1 x 1 x 1 } P 1 [ I C] P 1 [ I C] R P 1 [7] Si Llamamos P1 a P 1 y P1 P: P P1 CP PC CPC R rp rp 1 2rCP r CPC rr r traza d la matriz Difrnciamos rspcto a para obtnr l mínimo d la xprsión igualamos a cro s dcir: rp 2 rcp 2rCPC 2R 0 [ CPC ] 1 CP R [ CP 1 C R 1 ] 1 1 CP 1 [8] S pud comprobar qu s l mínimo a traés dl Hssiano. Substituyndo [8] n [7]: [ I C] P 1 P 1 [9] Inicialización dl algoritmo: Para iniciar l algoritmo s ncsario conocr las siguints condicions d contorno: Un alor inicial dl stado: x0. l alor d la matriz d coarianza dl rror, qu para P0. Podmos asumir qu P0 Q. Y los alors d las matrics d coarianza asociadas al sistma y a la mdida: Q y R

7 Rsumindo, l algoritmo tin los siguints pasos: n la itración : 1º Prdicción: x 1 xˆ Bu 2º Corrcción P 1 P Q [ CP 1 C R 1 ] 1 1 CP 1 obsración. y1 x ˆ 1 x 1 1 y 1 C x 1 P 1 I C P 1 [ ] [ ] Nóts qu l concpto d filtrado s dbido a qu los alors dl stado óptimo xˆ 1 y d la obsración s producn n l mismo instant

8 jmplo 1. Filtrado d una constant Supongamos qu qurmos stimar l alor d una constant qu al sr mdida tin una sri d dsiacions dbidas a la incrtidumbr dl aparato d mdida. l sistma srá ntoncs: x1 x Bu y Cx 1 B0 C1 x1 x y x Supongamos qu la dsiación típica d la mdida s σ La coarianza arianza n st caso srá σ R. Y considrmos también qu l sistma tin una 2 dsiación muy pquña σ La arianza asociada al sistma s σ 1 6. P n la siguint gráfica s pud obsrar l fcto dl filtro d alman para una constant d 30. x

9 Código utilizado para gnrar la gráfica:! " # $ %! " $ %$ $! " # " # & " #! " " & "! " " #! $ *, $ $ " -! " - " - "!.. - " - / " - - " #! 0 # 1 - $ - " # &! - " &1-2 " -! " - 2. & " -! jmplo 1. Filtrado dl stado para un sistma d sgundo ordn Supongamos l siguint sistma formado por un circuito RLC sri. Si analizamos l sistma n l dominio d Laplac, obtndrmos qu: 0 s s LCs i 2 1 RCs 1 Pasando a spacio d stados, nota: l sistma s continuo: - 9 -

10 x x 2 LC x y x 2 Sindo: 1 [ 1 0] 1 x 1 1 R i s x 1 0 L 2 LC x 1 0 x 2 ommos los alors sí pus: L 1 H. C 1000 µf R 30 d 0 dt ; B ; C [ 1 0] Si prsumimos un timpo d mustro d 0.01 s ; B ; C [ 1 0] Suponmos una dsiación dl obsrador d 1 oltio Suponmos una dsiación dl sistma d 0.01 oltio y 0.01 oltio/sgundo. σ ; σ ; σ 1 por lo qu : R 1; y Q n la siguint gráfica s pud r l rsultado d la stimación

11 Código utilizado para gnrar la gráfica: 3 2!4! $! 5 6$!2 /4 & 2 /4 7!8 6$! /4 & 7! 6 $ 7!3 $! :8 : :3! ; * ; $ %$ % < :$ %$! 6 :8 : :3 7 <! *, $ >! *,? $ >! 61 " : " 7 :8 : :3 :>! 6 : 7 "! " #! # " $ %$! # $ < $ / 6 " $!$ " 7! " #! "! $ 6$!$ 7! # *, " & - 8 &> - " &! " & " & B-! # 1 1 " - " # &!.. & " & & " & B-! " -. B&1 2 & "! C -!C -!< - 1! " < 2 B&. & "!

12 Filtro d alman con stimación: Partimos d un sistma linal rprsntado por las cuacions: x 1 x Bu [10] y Cx Como ya s indicó con antrioridad, la solución qu propusiron alman y Bucy para l prdictor dl stado, ndrá dtrminada por l stado n l instant antrior y por un factor d corrcción qu srá función dl rror ntr la obsración y la stimación. [ y yˆ ] xˆ 1 xˆ Bu [11] y ˆ Cxˆ sumidas todas las condicions inicials, análogas al caso d filtrado, s trata aquí d dtrminar la stimación x ˆ 1, conocindo las mdidas contaminadas d ruido y0, y1, y2,..., y, para qu la matriz P1 d coarianza dl rror, n l instant 1, sa mínima. l rror n l instant 1 s dtrminará por: 1 x 1 xˆ 1 [12] Substituyndo n [12] las xprsions [10] y [11] Llamando: tnmos: [ Cx Cxˆ ] 1 x Bu xˆ Bu [ xˆ ] C [ x xˆ ] 1 x [ x xˆ ] [ C] 1 ˆ C [13] 1 ˆ

13 sí pus la matriz d coarianza dl rror tal y como s había dfinido n [2] { } P Substituyndo y oprando: { [ ][ ] } ˆ ˆ { ˆ ˆ ˆ ˆ ˆ } ˆ y como: { } { } ˆ ˆ ˆ ˆ ˆ ˆ P { } { } 0 ˆ ˆ { } { } 0 ˆ ˆ { } { } R { } { } 0 { } Q Quda: Q R P P ˆ ˆ 1 nindo n cunta l alor d [ ] [ ] Q R C P C P 1 [14] Una z qu hmos calculado la xprsión d la matriz d coarianza dl rror, tnmos qu ncontrar l alor d qu hac qu sa matriz sa mínima: [ ] [ ] Q R C P C P 1 C CP CP C P P P 1 Q R

14 rp 1 r P r CP r CP C r R r Q Si difrnciamos rspcto a rp 1 2r CP 2r CP C Igualando a cro para buscar l mínimo: 2 r CP 2r CP C 2r R 0 l sr P y R simétricas: [ CP C R] 2CP 2r R [ CP C ] 1 CP R [15] Substituyndo st alor n [14] obtnmos P1 [ C] P P 1 Q [16] Rsumindo, l procso itratio a sguir s: 1.- Cálculo d la ganancia: [ CP C ] 1 CP R 2.- stimación dl stado: xˆ 1 xˆ Bu [ y Cxˆ ] 3.- ctualización d la matriz d coarianza dl rror [ C] P P 1 Q Nóts qu l concpto d stimación s dbido a qu cuando tnmos la obsración dl sistma y los alors obtnidos para l stado óptimo son los d x ˆ

15 Si rprsntamos l filtro n forma d bloqus, obtnmos la siguint rprsntación:

16 Documntación [1] Rafal Molina Soriano. Bass Dl Filtro d alman. [2] ma XVII l filtro d alman ING-C/ctr_az/VV17.PDF [3] Grg Wlch, Gary Bishop. Introduction to th alman Filtr. Ptr S. Haybc. Stochastic modls, stimation and control. Volum 1 Grg Wlch, Gary Bishop. SC. Incrmntal tracing ith Incomplt Information [4] alman Filtr Driation