[

as the state vector. Because A is constant, x = x° and D is the identity matrix. The observation vector consists of n values of 0„ i.e., y=[0„02--A]T

To construct the observation model vector, g, we express the 6i in terms of the elements of A by

0, = cos ~1 (U, • A) = cos ~~1 ( UXAX + UVA,, + U,At)

where U, be calculated at the time of the ith observation, i.e., U, = ¿¡(f,.). The elements of g are then given by g, = cos"'(t/^cosa cosfi + Uysmacos8+ UzsinS)

The (nX 2) matrix of partial derivatives of the observation model with respect to the state vector is

where

k and dg. i/J(cosasinS+ l/^sinasinS-i/zcosS

If the a priori state vector estimate, x^ =[a/(,5/,]T, is known to an estimated accuracy of [oa,o6] and all observations are measured with equal estimated accuracies of eg, then Eq. (13-35) gives the following solution for x, assuming that x^ and y are weighted according to their expected accuracies:

x,=

«0 Ï

+

0 0

Post a comment