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