Bilag 1: Helikopterens fysiske egenskaber - SmartData

Bilag 16: Altitude estimering med Kalman filter
Målingerne opsamles diskret og antages at kunne beskrives ved nedenstående ligning
hvor
z = H x + v
k
zk : målevektor
k
k
k
vk : målestøj for den benyttede sensor
Hk : matrice der relaterer sensorerne til tilstandsvektoren xk.
Vi kan skrive vores accelerometer måling som:
dx2
a m = + g + a
dt
n
2
Hvor an er normalt fordelt hvidstød n ( 0,
n ) N a σ
omskrives til:
= x
+ g + an
⇒ x
2
= am
− g − an
= u + w
am 2
≡ og g er tyngdeaccelerationen. Dette kan
Tilstandsligningen for systemet kan diskretiseres ved at benytte Taylor-udvikling
2
( τ − a)
p(
τ ) = f ( a)
+ f
( a)(
τ − a)
+ f
(
a)
+ .... hvor a = t og τ = t + ∆t
2!
Derved fås det diskrete system:
x1 k + 1)
= x1(
k)
+ x2
( k)
⋅ ∆t
+ 0.
5(
am
− g − an
( ) ∆t
x k + 1)
= x ( k)
+ 0.
5(
a − g − a
2 ( 2
m
n
Dette kan skrives på matrix form som:
⎡ x1(
k + 1)
⎤ ⎡1 ∆t⎤
⎡ x1(
k)
⎤ ⎡0.
5∆
⎢ ⎥ = ⎢ ⎥ ⋅ ⎢ ⎥ + ⎢
⎣x2
( k + 1)
⎦ ⎣0
1 ⎦ ⎣x
2 ( k)
⎦ ⎣ 0.
5
)
2
2
t ( am
− g)
( a − g)
m
2
2
⎤ ⎡−
0.
5∆t
a
⎥ + ⎢
⎦ ⎣ − 0.
5an
⎤
⎥
⎦
n