Page 201 - Innovations in Intelligent Machines

State Estimation for Micro Air Vehicles 193
We can compute the evolution for P as
d T
˙
P = E{˜x˜x }
dt
˙ T ˙ T
= E{˜x˜x +˜x˜x }
T T T T T T
= E A˜x˜x + Gξ˜x +˜x˜x A +˜xξ G
T
T T
T
T
= AP + PA + GE{ξ˜x } + E{˜xξ }G ,
where

T t T +
T A (t−τ)
T
T
E{ξ˜x } = E ξ(t)˜x 0 e A t + ξ(t)ξ (τ)G e dτ
0
1 T
= QG ,
2
which implies that
˙
T
T
P = AP + PA + GQG .
At Measurements.
At a measurement we have that
+
˜ x = x − ˆx +

−
= x − ˆx − L Cx + η − Cˆx −
−
−
=˜x − LC˜x − Lη.
Therefore
+ +T
P + = E{˜x ˜x }
, -
T
= E ˜ x − LC˜x − Lη ˜ x − LC˜x − Lη
−
−
−
−
− −T − −T T T − T T
= E ˜x ˜x − ˜x ˜x C L − ˜x η L
T
− −T
T
− T
− −T
− LC˜x ˜x + LC˜x ˜x C L + LC˜x η L T
T
T
T
= −Lη˜x −T + Lη˜x −T C L + Lηη L T
T
T
T
T
T
−
−
= P − − P C L − LCP − + LCP C L + LRL . (25)
+
Our objective is to pick L to minimize tr(P ). A necessary condition is
∂ + T T T
−
−
tr(P )= −P C − P C +2LCP C +2LR =0
−
∂L
T
−
=⇒ 2L(R + CP C )=2P C T
−
T
T −1
−
=⇒ L = P C (R + CP C ) .
−