Lecture 9

advertisement
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
Lecture 9
Last time: Linearized error propagation
es = Se1
Integrate the errors at deployment to find the error at the surface.
Es = es esT
= S e1 e1T S T
= SE1S T
Or Φ can be integrated from:
& = F Φ, where Φ (0) = I
Φ
x& = f ( x )
F=
df
dx
where F is the linearized system matrix. But this requires the full Φ (same
number of equations as finite differencing).
tn = time when the nominal trajectory impacts.
e (tn ) = Φ (tn ) e1
er (tn ) = e2 = Φ r e1
where Φ r is the upper 3 rows of Φ (tn ) .
Covariance matrix:
E2 = Φ r E1Φ Tr
Page 1 of 8
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
e& = Fe
E ( t ) = e ( t ) e ( t )T
E& (t ) = e& (t ) e (t )T + e (t ) e& (t )T
= F e ( t ) e ( t )T + e ( t ) e ( t ) T F T
= FE (t ) + E (t ) F T
You can integrate this differential equation to tn from E (0) = E1 . This requires the
full 6 × 6 E matrix.
⎡ er erT er evT ⎤
E ( tn ) = ⎢
⎥
⎢⎣ ev erT ev evT ⎥⎦
E2 = upper left 3 × 3 partition of E (tn )
For small times around tn,
e (t ) = e (tn ) + v (tn )(t − tn )
= e (tn ) + ( vn (tn ) + ev (tn ))(t − tn )
= e (tn ) + vn (tn )(t − tn )
1vT e (t ) = 1vT e2 + 1vT vn (tn )(t − tn ) = 0
(ti − tn ) = −
1vT e2
1vT vn
Page 2 of 8
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
e3 = position error at impact
= e2 − vn
1vT e2
1vT vn
⎡
v 1T ⎤
= ⎢ I − nT v ⎥ e2
1v vn ⎦
⎣
"projection matrix"
e3′ = [ R ]e2
⎡cos θij ... ...⎤
R = ⎢ ...
... ...⎥
⎢
⎥
... ...⎥⎦
⎢⎣ ...
Rij = cos θij
R = [ 11
12
13 ]
1j = unit vectors along the jth axis of the 2 frame expressed in the coordinates of
the 3 frame.
e3′ = Re2
E3′ = RE2 RT
Page 3 of 8
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
eR (t ) = eR3 + vn cos γ (t − tn )
eT (t ) = eT3
eH (t ) = eH 3 − vn sin γ (t − tn )
Impact:
eH (ti ) = eH 3 − vn sin γ (ti − tn ) = 0
(ti − tn ) =
1
eH
vn sin γ 3
eR (ti ) = eR3 +
vn cos γ
eH
vn sin γ 3
= eR3 + cot γ eH 3
eT (ti ) = eT3
The transformation which relates R,H,T errors at the nominal end time to R and T
errors when H=0 is:
⎡ e (t ) ⎤
e4 = ⎢ R i ⎥
⎣ eT (ti ) ⎦
⎡ eR3 + cot γ eH 3 ⎤
=⎢
⎥
eT3
⎣⎢
⎦⎥
⎡1 0 cot γ ⎤
e3′ ≡ Pe3′
=⎢
0 ⎥⎦
⎣0 1
If the es defined earlier, based on integration of perturbed trajectories, is
measured in R,T coordinates, then the sensitivity matrix defined at that point is
equivalent to
es = Se1
S = PRΦ r
Page 4 of 8
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
E4 = PE3′ PT
⎡ R 2 RT ⎤ ⎡ σ 2
=⎢
⎥=⎢ R
⎢⎣TR T 2 ⎥⎦ ⎣ µRT
µ RT ⎤ ⎡ σ R2
⎥=⎢
σ T2 ⎦ ⎣ ρσ Rσ T
ρσ Rσ T ⎤
⎥
σ T2 ⎦
If all the original error sources are assumed normal, R and T will have a joint
binormal distribution since they are derived from the error sources by linear
operations only. This joint probability density function is
f ( r, t ) =
1
2πσ Rσ T 1 − ρ 2
2
⎡ ⎛ r ⎞2
⎛ r ⎞⎛ t ⎞ ⎛ t ⎞ ⎤
⎢⎜
⎟ −2 ρ ⎜
⎟⎜
⎟+⎜
⎟ ⎥
⎢ σ
⎝ σ R ⎠⎝ σ T ⎠ ⎝ σ T ⎠ ⎥
−⎢ ⎝ R ⎠
⎥
2 1− ρ 2
⎢
⎥
⎢
⎥
⎣
⎦
(
e
)
where σ R , σ T and ρ can be identified from E4 . Recall that we are considering
unbiased errors.
Contour of constant probability density function is
2
2
⎛ r ⎞
⎛ r ⎞⎛ t ⎞ ⎛ t ⎞
2
⎜
⎟ − 2ρ ⎜
⎟⎜
⎟+⎜
⎟ =c
σ
σ
σ
σ
⎝ R⎠
⎝ R ⎠⎝ T ⎠ ⎝ T ⎠
r = x cos θ − y sin θ
t = x sin θ + y cos θ
Get:
(θ ) x 2 + ({
θ ) xy + (θ ) y 2 = c 2
=0
Coefficient of x, y equals zero for principal axes.
2 ρσ σ
2µ
tan 2θ = 2 R T2 = 2 RT 2
σ R − σT σ R − σT
Use a 4 quadrant tan-1 function.
Page 5 of 8
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
Once θ is found, can plug into pdf expression, get σ x and σ y .
h = (σ R2 − σ T2 ) 2 + (2 ρσ Rσ T ) 2
1
2
1
σ y2 = (σ R2 + σ T2 − h )
2
σ x2 = (σ R2 + σ T2 + h )
xi = cσ x cos φi
yi = cσ y sin φi
May want to choose c to achieve a certain probability of lying in that contour.
In principal coordinates, the probability of a point inside a “ cσ ” ellipse is
P = 1− e
−
c2
2
People often choose c to find what is called the circular probable error (CPE).
Page 6 of 8
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
1
2
σ = (σ x + σ y )
Choosing P=0.5, c=1.177
CPE = 0.588(σ x + σ y )
This approximation is good to an ellipticity of around 3.
Random Processes
A random process is an ensemble of functions of time which occur at random.
In most instances we have to imagine a non-countable infinity of possible
functions in the ensemble.
There is also a probability law which determines the chances of selecting the
different members of the ensemble.
We generally characterize random processes only partially.
One important descriptor – the first order distribution.
This is the classical description of random processes. We will also give the state
space description later.
Page 7 of 8
16.322 Stochastic Estimation and Control, Fall 2004
Prof. Vander Velde
x (t1 ) is a random variable.
F ( x, t ) = P [ x (t ) ≤ x ] , where x (t ) is the name of a process and x is the value taken
f ( x, t ) =
dF ( x, t )
dx
Page 8 of 8
Download