Filtro di Kalman Per stimare il Vento

[dimaios]

Si, giovane jedi, convertiti al lato oscuro della forza .......

[minidiable]

ahhahahaahahhahahaha....

Ci provo e ti faccio sapere :)

[minidiable]

Allora ho i primi problemi

Il filtro di Kalman originale (quello che veniva usato prima che io arrivassi) era usato per trovare dalle misure di posizione quelle di velocita' (non ho capito bene perche' non bastava una semplice differenziazione...ma lasciamo perdere questo aspetto per ora).

In uscita al filtro di Kalman c'era il vettore di stato che conteneva:
posizione e velocita' rispetto a x y z . Quindi era composto da 6 componenti.

Per tenere in conto il vento ho pensato di estendere il vettore di stato, come detto gia' in precedenza, e far si che il mio filtro oltre a stimare le 6 variabili precedenti (ovvero posizione e velocita'), ne stimava altre 3 che sarebbero dovute essere "proporzionali" al vento.

Come ho fatto?

Tutto l'algoritmo del filtro era basato su un equilibrio di accelerazioni:



Per tenere in conto il vento ho modificato l'equazione in questo modo (per provare a simulare il vento):

In questo modo nel filtro decidevo io quali segnali windx windy windz fare entrare.

Se provavo con segnali costanti il mio filtro di Kalman riusciva a stimarli e quindi la cosa mi sembrava funzionare...

Provavo con segnali costanti perche' molto semplicisticamente se penso a un vento diretto nel piano penso a una forza costante che mi porta il quadricottero ad essere stabile in un'altra posizione.

Detto questo ho provato a fare lo stesso sul quadricottero reale, ma naturalmente non sembra funzionare in quanto se io TIRO letteralmente il quadricottero e me lo porto in una posizione diversa da quella voluta lui riesce a mantenersi in equilibrio (naturalmente inclinandosi) ma quello che esce dal filtro di Kalman non e' un segnale costante, ma semplicemente una derivata della velocita' del quadricottero..
Quindi quando lo tiro il segnale cresce, per poi decrescere, andare sotto lo zero e ritornare a zero nel momento in cui smetto di tirarlo. (Allego una figura in cui si vede quello di cui parlo...il picco 1 corrisponde a quando io tiro il quadricottero...Il 2 quando sto per fermarmi , mi scuso per non postare l'immagine in fidocadj ma era solo per farvi capire quello di cui parlavo)

Non capisco, sicuramente ci sta un errore concettuale grave, o qualche errore di implementazione nel filtro di Kalman. In questo momento mi serve un aiuto.

Qualcuno puo' fornirmelo?