SWISS FEDERAL INSTITUTE OF TECHNOLOGY LAUSANNE EIDGENÖSSISCHE TECHNISCHE HOCHSCHULE LAUSANNE POLITECNICO FEDERALE DI LOSANNA DÉPARTEMENT DE MICROTECHNIQUE INSTITUT DE SYSTÈMES ROBOTIQUE Autoomous Systems Lab Prof. Dr. Rolad Segwart A Itroducto To Error Propagato: Dervato, Meag ad Examples T of Equato F X C X F X C Y Ka Olver Arras Techcal Report Nº EPFL-ASL-TR-98-0 R3 of the Autoomous Systems Lab, Isttute of Robotc Systems, Swss Federal Isttute of Techology Lausae (EPFL) September 998
Cotets. Itroducto............................................ Error Propagato: From the Begg........................ A Frst Expectato........................................... 3. Whe s the Approxmato a Good Oe?........................... 4.3 The Almost Geeral Case: Approxmatg the Dstrbuto of Y f( X, X,, X )......................................... 5.3. Addedum to Chapter................................... 6.4 Gettg Really Geeral: Addg Z g( X, X,, X ).................. 6 3. Dervatg the Fal Matrx Form........................... 7 4. Examples............................................. 9 4. Probablstc Le Extracto From Nosy D Rage Data............... 9 4. Kalma Flter Based Moble Robot Localzato: The Measuremet Model...................................... 5. Exercses............................................ 3 Lterature............................................. 4 Appedx A Fdg the Le Parameters r ad α the Weghted Least Square Sese................................ 5 Appedx B Dervg the Covarace Matrx for r ad α................ 7 B. Practcal Cosderatos...................................... 9
. Itroducto Ths report attempts to shed lght oto the equato C Y T F X C X F X, () where C X s a, C Y a p p covarace matrx ad F X some matrx of dmeso p. I estmato applcatos lke Kalma flterg or probablstc feature extracto we frequetly ecouter the patter F X C X F X. May texts lterature troduce ths equato wth- T out further explaato. But relatoshp (), called the error propagato law, ca be explctely derved ad uderstood, beg mportat for a compreheso of ts uderlyg approxmatve character. Here, we wat to brdge the gap betwee these texts ad the ovce to the world of ucertaty modelg ad propagato. Applcatos of () are e.g. error propagato model-based vso, Kalma flterg, relablty or probablstc systems aalyss geeral.. Error Propagato: From the Begg We wll frst forget the matrx form of () ad chage to a dfferet perspectve. Error propagato s the problem of fdg the dstrbuto of a fucto of radom varables. Ofte we have mathematcal models of the system of terest (the output as a fucto of the put ad the system compoets) ad we kow somethg about the dstrbuto of the put ad the compoets. X System Y Fgure : The smplest case: oe put radom varable ( N ), ad oe output radom varable ( P ). The, we desre to kow the dstrbuto of the output, that s the dstrbuto fucto of Y whe Y f( X) where f (). s some kow fucto ad the dstrbuto fucto of the radom varable X s kow. If f (). s a olear fucto, the probablty dstrbuto fucto of Y, p Y ( y), becomes quckly very complex, partcularly whe there s more tha oe put varable. Although a geeral method of soluto exsts (see [BREI70] Chapter 6-6), ts complexty ca be demostrated already for smple problems. A approxmato of p Y ( y) s therefore desrable. The approxmato cossts the propagato of oly the frst two statstcal momets, that s the mea µ Y ad the secod (cetral) momet σ Y, the varace. These momets do ot geeral descrbe the dstrbuto of Y. However f Y s assumed to be ormally dstrbuted they do. We do ot cosder pathologcal fuctos f (). where Y s ot a radom varable however, they exst That s smply oe of the favorable propertes of the ormal dstrbuto.
. A Frst Expectato Look at fgure where the smple case wth oe put ad oe output s llustrated. Suppose that X s ormally dstrbuted wth mea µ ad stadard devato X σ X. Now we would lke Y µ y + σ y f( X) µ y µ y σ y µ x σ x µ x µ x + σ x X Fgure : Oe-dmesoal case of a olear error propagato problem to kow how the 68% probablty terval [ µ X σ X, µ X + σ X ] s propagated through the system f ().. Frst of all, from fgure t ca be see that f the shaded terval would be mapped oto the y -axs by the orgal fucto ts shape would be somewhat dstorted ad the resultg dstrbuto would be asymmetrc, certaly ot Gaussa aymore. Whe approxmatg f( X) by a frst-order Taylor seres expaso about the pot X µ X, Y f ( µ X ) + f X µ X ( X ), () we obta the lear relatoshp show fgure ad wth that a ormal dstrbuto for p Y ( y). Now we ca determe ts parameters µ Y ad σ Y. µ Y f ( µ X ), (3) σ Y f σ X X µ X. (4) Fally the expectato s rsed that the remader of ths text we wll aga bump to some geeralzed form of equato (3) ad (4). At ths pot, we should ot forget that the output dstrbuto, represeted by µ Y ad σ Y, s a approxmato of some ukow truth. Ths truth s mpertetly olear, o-ormal ad asymmetrc, thus hbtg ay exact closed form aalyss most cases. We are the supposed to ask the questo: µ X Remember that the stadard devato σ s by defto the dstace betwee the most probable value, µ, ad the curve s turg pots. Aother useful property of the ormal dstrbuto, worth to be remembered: Gaussa stays Gaussa uder lear trasformatos. 3
. Whe s the Approxmato a Good Oe? Some textbooks wrte equatos (3) ad (4) as equaltes. But f the left had sdes deote the parameters of the output dstrbuto whch s, by assumpto, ormal, we ca wrte them as equaltes. The frst two momets of the true (but ukow) output dstrbuto let s call them µ ad are deftely dfferet from these values 0 σ 0. Hece µ 0 µ Y σ 0 σ Y It s evdet that f f (). s lear we ca wrte (5) ad (6) as equaltes. However the followg factors affect approxmato qualty of µ Y ad σ Y by the actual values y ad s y. µ Y y σ Y s y Thus they apply for both cases; whe f (). s lear ad whe t s olear. The guess x : I geeral we do ot kow the expected value µ X. I ths case, ts best (ad oly avalable) guess s the actual value of X, for example the curret measuremet x. We the dfferetate f( X) at the pot X x hopg that x s close to E[ X] such that y s ot too far from E[ Y]. Extet of olearty of f (). : Equatos (3) ad (4) are good approxmatos as log as the frst-order Taylor seres s a good approxmato whch s the case f f (). s ot too far from lear wth the rego that s wth oe stadard devato of the mea [BREI70]. Some people eve ask f (). to be close to lear wth a ± σ -terval. The olearty of f (). ad the devato of x from E[ X] have a combed effect o σ Y. If both factors are of hgh magtude, the slope at X x ca dffer strogly ad the resultg approxmato of σ Y ca be poor. At the other had, f already oe of the them s small, or eve both, we ca expect s y to be close to σ Y. The guess s x : There are several possbltes to model the put ucertaty. The model could corporate some ad hoc assumptos o σ X or t mght rely o a emprcally gaed relatoshp to µ X. Sometmes t s possble to have a physcally based model provdg true ucertaty formato for each realzato of X. By systematcally followg all sources of perturbato durg the emergece of x, such a model has the desrable property that t accouts for all mportat factors whch fluece the outcome x ad s x. I ay case, the actual value s x remas a guess of σ X whch s hopefully close to σ X. But there s also reaso for optmsm. The codtos suggested by Fgure are exaggerated. Mostly, σ X s very small wth respect to the rage of X. Ths makes () a approxmato of suffcet qualty may practcal problems. (5) (6) (7) (8) Remember that the expected value ad the varace (ad all other momets) have a geeral defto,.e. are depedet whether the dstrbuto fuctos exhbts some ce propertes lke symmetry. They are also vald for arbtrarly shaped dstrbutos ad always quatfy the most probable value ad ts spread. It mght be possble that some olear but ce-codtoed cases the approxmato effects of oormalty ad the other factors compesate each other yeldg a very good approxmato. Nevertheless, the opposte mght also be true. 4
.3 The Almost Geeral Case: Approxmatg the Dstrbuto of Y f( X, X,, X ) The ext step towards equato () s aga a practcal approxmato based o a frst-order Taylor seres expaso, ths tme for a multple-put system. Cosder X X X 3 System Y X Fgure 3: Error propagato a mult-put sgle-output system: N, P. Y f( X, X,, X ) where the X s are put radom varables ad Y s represeted by ts frst-order Taylor seres expaso about the pot µ, µ,, µ Equato (9) s of the form Y f ( µ, µ,, µ ) + f ( µ. (9), µ,, µ ) [ X µ ] Y a 0 + a ( X µ ) wth a 0 f ( µ, µ,, µ ), (0) a f ( µ, µ,, µ ). () As chapter., the approxmato s lear. The dstrbuto of Y s therefore Gaussa ad we have to determe µ Y ad σ Y. µ Y E[ Y] E[ a 0 + a ( X µ )] () E[ a 0 ] + E[ a X ] E[ a µ ] (3) a 0 + a E[ X ] a E [ µ ] (4) a 0 + a µ a µ (5) a 0 (6) µ Y f ( µ, µ,, µ ) (7) σ Y E[ ( Y µ Y ) ] E[ ( a ( X µ )) ] (8) E[ a ( X µ ) a j ( X j µ j )] (9) j E[ a ( X µ ) + a a j ( X µ )( X j µ j )] (0) j 5
a E[ ( X µ ) ] + a a j E[ ( X µ )( X µ j )] () a σ + a a j σ j () j σ f Y σ f f + σj j j (3) The vector µ, µ,, µ has bee omtted. If the X s are depedet the covarace σ j dsappears, ad the resultg approxmated varace s σ f Y σ. (4) Ths s the momet to valdate our expectato from chapter. for the oe-dmesoal case. Equato (7) correspods drectly wth (3) whereas (3) somewhat cotas equato (4)..3. Addedum to Chapter. I order to close the dscusso o factors affectg approxmato qualty, we have to cosder brefly two aspects whch play a role f there s more tha oe put. Idepedece of the puts: Equato (4) s a good approxmato f the stated assumpto of depedece of all s s vald. X It s fally to be metoed that, eve f the put dstrbutos are ot strctly Gaussa, the assumpto of the output beg ormal s ofte reasoable. Ths follows from the cetral lmt theorem whe the s somehow addtvely costtute the output Y. X j.4 Gettg Really Geeral: Addg Z g( X, X,, X ) Ofte there s ot just a sgle Y whch depeds o the X s but there are more system outputs, that s, more radom varables lke Y. Suppose our system has a addtoal output Z wth Z g( X, X,, X ). X X X 3 X System Y Z Fgure 4: Error propagato a mult-put mult-output system: N, P Obvously µ Z ad σ Z ca exactly be derved as show before. The addtoal aspect whch s troduced by Z s the questo of the statstcal depedece of Y ad Z whch s expressed by ther covarace σ YZ E[ ( Y µ Y )( Z µ Z )]. Let s see where we arrve whe substtutg Y ad Z by ther frst-order Taylor seres expaso (9). 6
σ YZ E[ ( Y µ Y )( Z µ Z )] (5) E[ Y Z] E[ Y]E[ Z] (6) E f µ Y + [ X (7) µ ] g µ Z + [ X µ ] µ Y µ Z f g f g E µ Y µ Z + µ Z [ X (8) µ ] + µ Y [ X µ ] + [ X µ ] [ X µ ] µ Y µ Z f f E [ µ Y µ Z ] + µ Z E µ + µ Y E + E X f g [ X j µ ][ X j µ j ] f µ Y µ Z µ Z E X f [ ] µ Z E µ g [ ] µ Y E X g + + [ ] µ Y + E µ Y µ Z g X g µ f g [ X µ ] f g + [ X j µ ][ X j µ j ] µ Y µ Z j E [ µ ] (9) (30) σ YZ X f g (3) E ( X µ ) f g [ ] + E [( X j µ )( X j µ j )] j f g f g σ (3) + σ j X j j j If ad are depedet, the secod term, holdg ther covarace, dsappears. Addg more output radom varables brgs o ew aspects. I the remader of ths text we shall cosder P wthout loss of geeralty. 3. Dervatg the Fal Matrx Form Now we are ready to retur to equato (). We wll ow see that we oly have to reformulate equatos (3) ad (3) order to obta the tal matrx form. We recall the gradet operator wth respect to the -dmesoal vector X X. (33) f ( X ) s a p -dmesoal vector-valued fucto f ( X ) f ( X ) f ( X ) f p ( X ) T. The Jacoba F X s defed as the traspose of the gradet of f ( X ), whereas the gradet s the outer product of ad f ( X ) X T F X X f ( X ) T T f ( X ) ( X ) f f f f f (34) F X has dmeso ths case, p geeral. We troduce the symmetrc put covarace matrx C X whch cotas all varaces ad covaraces of the put radom varables X, X,, X. If the X s are depedet all σ j wth j dsappear ad C X s dagoal. 7
σ X σ X X σ X X C X σ X X σ X : : : σ X X σ X X σ X σ X X (35) We further troduce the symmetrc output covarace matrx C Y ( p p geeral wth outputs Y, Y,, Y p ) C Y σ Y σ Y Y σ Y σ Y Y (36) Now we ca statly form equato () C Y F X C X F X T (37) σ Y σ Y Y σ Y Y σ Y f f f f σ X σ X X σ X X σ X X σ X σ X X : : : σ X X σ X X σ X f f : : f f (38) f σx f σx f + + σx X X f + + σx X X f f σx X + + σx X f f σx X + + σx X f f : : f f (39) Looks ce. But what has t to do wth chapter? To aswer ths questo we evaluate the frst elemet, the varace of : σ Y σ Y Y f f f f f f f f f f σ X + σx X + + σx X X + σx X + σ X + + σx X X f f f f f + + σx X X + σx X + + σ X (40) f f f σ X + σx X j j j (4) If we ow retroduce the otato of chapter.3, that s, f ( X ) f( X), Y Y, ad σ X σ, we see that (4) equals exactly equato (3). Assumg the reader beg a otorous skeptc, we wll also look at the off-dagoal elemet σ Y Y, the covarace of Y ad Z: f σ Y Y f f f f f f f f f f f σx + σx X + + σx X X + σx X + σx + + σx X X f f f f f + + σx X X + σx X + + X f σx (4) 8
f f f f σx + (43) σx X j j j Aga, by substtutg f ( X ) by f( X), f ( X ) by g( X) ad σ X by σ, equato (43) correspod exactly to the prevously derved equato (3) for σ YZ. We were obvously able, havg started from a smple oe-dmesoal error propagato problem, to derve the error propagato law C Y T F X C X F X. Puttg the results together yelded ts wdely used matrx form (). Now we ca also uderstad the formal terpretato of fgure 5. The put ucertaty... T C Y F X C X F X...ad approxmatvely mapped to the output....s popagated through the system f(.)... Fgure 5: Iterpretato of the error propagato law ts matrx form 4. Examples 4. Probablstc Le Extracto From Nosy D Rage Data Model-based vso where geometrc prmtves are the sought mage features s a good example for ucertaty propagato. Suppose the segmetato problem has already bee solved, that s, the set of ler pots wth respect to the model s kow. Suppose further that the regresso equatos for the model ft to the pots have a closed-form soluto whch s the case whe fttg straght les. Ad suppose fally, that the measuremet ucertates of the data pots are kow as well. The we ca drectly apply the error propagato law order to get the output ucertates of the le parameters. x (r, q ) r d α Fgure 6: Estmatg a le the least squares sese. The model parameters r (legth of the perpedcular) ad α (ts agle to the abscssa) descrbe uquely a le. 9
Suppose measuremet pots polar coordates x ( ρ, θ ) are gve ad modeled as radom varables X ( P, Q ) wth,,. Each pot s depedetly affected by Gaussa ose both coordates. P ~ N( ρ, σ ρ ) (44) Q ~ N( θ, σ θ ) (45) E[ P P j ] E[ P ]E[ P j ], j,, (46) E[ Q Q j ] E[ Q ]E[ Q j ], j,, (47) E[ P Q j ] E[ P ]E[ Q j ], j,, (48) Now we wat to fd the le x cosα + y sα r 0 where x ρcos( θ) ad y ρs( θ) yeldg ρcosθcosα + ρsθsα r 0 ad wth that, the le model ρcos( θ α) r 0. (49) Ths model mmzes the orthogoal dstaces from the pots to the le. It s mportat to ote that fttg models to data some least square sese yelds ot a satsfyg geometrc soluto geeral. It s crucal to kow whch error s mmzed by the ft equatos. A good llustrato s the paper of [GAND94] where several algorthms for fttg crcles ad ellpses are preseted whch mmze algebrac ad geometrc dstaces. The geometrc varety of solutos for the same set of pots demostrate the mportace of ths kowledge f geometrc meagful results are requred. The orthogoal dstace d of a pot ( ρ, θ ) to the le s just ρ cos( θ α) r d. (50) Let S be the (uweghted) sum of squared errors. The model parameters ( α, r) S d ( ρ cos( θ α) r) are ow foud by solvg the olear equato system (5) 0. (5) Suppose further that for each pot a varace modellg the ucertaty radal ad agular drecto s gve a pror or ca be measured. Ths varace wll be used to determe a weght for each sgle pot, e.g. w The, equato (5) becomes S 0 S w σ σ. (53) S w d w ( ρ cos( θ α) r). (54) The ssue of determg a adequate weght whe σ (ad perhaps some addtoal formato) s gve s complex geeral ad beyod the scope of ths text. See [CARR88] for a careful treatmet. 0
It ca be show (see Appedx A) that the soluto of (5) the weghted least square sese s α w ρ sθ ------ w w j ρ ρ j cosθ sθ j w --ata ------------------------------------------------------------------------------------------------------------------ w ρ cosθ ------ w w j ρ ρ j cos( θ + θ j ) w (55) r w ρ cos( θ α) ---------------------------------------------. (56) w Now we would lke to kow how the ucertates of the measuremets propagate through the system (55), (56). See fgure 7. X X X 3 X Model Ft A R Fgure 7: Whe extractg les from osy measuremet pots X, the model ft module produces le parameter estmates, modeled as radom varables A, R. It s the terestg to kow the varablty of these parameters as a fucto of the ose at the put sde. Ths s where we smply apply equato (). We are lookg for the matrx output covarace σ AR σ C A AR σ AR σ R, (57) gve the put covarace matrx C C P 0 dag( σ ρ ) 0 X 0 C Q 0 dag( σ θ ) (58) ad the system relatoshps (55) ad (56). The by calculatg the Jacoba P F P P PQ P P P Q Q Q Q Q Q (59) We follow here the otato of [DRAP88] ad dstgush a weghted least squares problem f C X s dagoal (put errors are mutually depedet) ad a geeralzed least squares problem f C X s o-dagoal.
we ca statly form the error propagato equato (60) yeldg the sought. C AR C AR T F PQ C X F PQ (60) Appedx A s cocered about the step-by-step dervato of the ft equatos (55) ad (56), whereas Appedx B equato (60) s oce more derved. Uder the assumpto of eglgble agular ucertates, mplemetable expressos for the elemets of C AR are determed, also a step-by-step maer. 4. Kalma Flter Based Moble Robot Localzato: The Measuremet Model The measuremet model a Kalma flter estmato problem s aother place where we ecouter the error propagato law. The reader s assumed to be more or less famlar wth the cotext of moble robot localzato, Kalma flterg ad the otato used [BAR93] or [LEON9]. I order to reduce the ubouded growth of odometry errors the robot s supposed to update ts pose by some sort of exteral referecg. Ths s acheved by matchg predcted evromet features wth observed oes ad estmatg the vehcle pose some sese wth the set of matched features. The predcto s provded by a a pror map whch cotas the posto of all evromet features global map coordates. I order to establsh correct correspodece of observed ad stored features, the robot predcts the posto of all curretly vsble features the sesor frame. Ths s doe by the measuremet model ẑ( k + k) h( xˆ ( k + k) ) + wk ( + ). (6) The measuremet model gets the predcted robot pose xˆ ( k + k) as put ad asks the map whch features are vsble at the curret locato ad where they are supposed to appear whe startg the observato. The map evaluates all vsble features ad returs ther trasformed postos the vector ẑ( k + k). The measuremet model s therefore a world-to-robot-tosesor frame trasformato. However, due to osystematc odometry errors, the robot posto s ucerta ad due to mperfect sesors, the observato s ucerta. The former s represeted by the (predcted) state covarace matrx Pk ( + k) ad the latter by the sesor ose model wk ( + ) N ( 0, Rk ( + ) ). They are assumed to be depedet. Sesg ucertaty Rk ( + ) affects the observato drectly, whereas vehcle posto ucertaty Pk ( + k) whch s gve world map coordates wll propagate through the frame trasformatos world-to-robot-to-sesor h ()., learzed about the predcto xˆ ( k + k). The the observato s made ad the matchg of predcted ad observed features ca be performed the sesor frame yeldg the set of matched features. The remag posto ucertaty of the matched features gve all observatos up to ad cludg tme k, Sk ( + ) cov[ z( k + ) Z k ], (6) Note that h (). s assumed to be tellget, that s, t cotas both, the mappg xˆ ( k + k) m j (wth m j as the posto vector of feature umber j ) ad the world-to-sesor frame trasformato of all vsble m j for the curret predcto xˆ ( k + k). Ths s cotrast to e.g. [LEON9], where solely the frame trasformato s doe by h( xˆ ( k + k), m j ) ad the mappg xˆ ( k + k) m j s somewhere else.
s the supermposed ucertaty of observato ad the propagated oe from the robot pose, Sk ( + ) Sk ( + ) hpk ( + k) h + Rk ( + ). (63) s also called measuremet predcto covarace or ovato covarace. 5. Exercses As stated the troducto, the report has educatoal purposes ad accompaes a lecture o autoomous systems the mcroegeerg departemet at EPFL. Thus, the audece of ths report are people ot yet too famlarzed wth the feld of ucertaty treatmet. Some propostos for exercses are gve: Let them do the dervato of µ Y (equatos () to (7)) ad σ Y (equatos (8) to (3)) gve a few rules for the expected value. Let them do the dervato of σ YZ (equatos (5) to (3)) or, the cotext of example, equatos (93) to (0) of Appedx B for σ AR. Some rules for the expected value ad double sums mght be helpful. Let them make a llustrato of each factor affectg approxmato qualty dscussed chapter. wth drawgs lke fgure. If least squares estmato a more geeral sese s the ssue, dervatg the ft equatos for a regresso problem s qute structve. The stadard case, lear the model parameters, ad wth ucertates oly oe varable s much smpler tha the dervato of example Appedx A. Addtoally the output covarace matrx ca be determed wth (). 3
Lterature [BREI70] [GAND94] [DRAP88] [CARR88] [BAR93] [LEON9] [ARRAS97] A.M. Brepohl, Probablstc Systems Aalyss: A Itroducto to Probablstc Models, Decsos, ad Applcatos of Radom Processes, Joh Wley & Sos, 970. W. Gader, G. H. Golub, R. Strebel, Least-Squares Fttg of Crcles ad Ellpses, BIT, vol. 34, o. 4, p. 558-78, Dec. 994. N.R. Draper, H. Smth, Appled Regresso Aalyss, 3rd edto, Joh Wley & Sos, 988. R. J. Carroll, D. Ruppert, Trasformato ad Weghtg Regresso, Chapma ad Hall, 988. Y. Bar-Shalom, X.-R. L, Estmato ad Trackg: Prcples, Techques, ad Software, Artech House, 993. J.J. Leoard, H.F. Durrat-Whyte, Drected Soar Sesg for Moble Robot Navgato, Kluwer Academc Publshers, 99. K.O. Arras, R.Y. Segwart, Feature Extracto ad Scee Iterpretato for Map- Based Navgato ad Map Buldg, Proceedgs of SPIE, Moble Robotcs XII, Vol. 30, p. 4-53, 997. 4
Appedx A: Fdg the Le Parameters r ad α the Weghted Least Square Sese Cosder the olear equato system S 0 (64) S 0 (65) where S s the weghted sum of squared errors S w ( ρ cosθ cosα + ρ sθ sα r). (66) We start solvg the system (64), (65) by workg out parameter r. S 0 w ( ρ cosθ cosα + ρ sθ sα r) ( ) (67) w ρ ( cosθ cosα + sθ sα) + w r (68) w ρ cos( θ α) + r w (69) rw w ρ cos( θ α) (70) w ρ r cos( θ α) ----------------------------------------------- (7) w Parameter α s slghtly more complcated. We troduce the followg otato cosθ c sθ s. (7) S w ( ρ c cosα + ρ s sα r) ( ρ c (73) cosα + ρ s sα r) w ρ c cosα + ρ s sα -------- w j ρ j cos( θ j α) ρ (74) c sα + ρ s cosα r w j w ρ c cosα + ρ s sα -------- w j ρ j cos( θ j α) ρ s (75) cosα ρ c sα -------- w j ρ j s( θ j α) w j w j w ρ c s cos α ρ c cosα sα -------- ρ c cosα w j ρ j s( θ j α) + ρ s cosα sα w j ρ s c s α -------- ρ w s sαw j ρ j j s( θ j α) -------- w w j ρ j j cos( θ j α) ρ s cosα + -------- w w j ρ j j cos( θ j α) ρ s cosα + ---------------- ( w j ) w j ρ j cos( θ j α) w j ρ j s( θ j α) w ρ c s cos α s ( α) ρ cosα sα( c s ) -------- w j ρ ρ j c cosα s( θ j α) -------- w w j ρ j ρ j s sα s( θ j α) -------- w w j ρ j ρ j s cosα cos( θ j α) + -------- w w j ρ j ρ j c sα cos( θ j α) + ---------------- ( w j ) w j w k ρ j ρ k cos( θ j α) s( θ k α) j k w j (76) (77) 5
cosα w ρ c s cosα sα w ρ c + -------- w w w j ρ ρ j ( c cosα + s sα) ( s j cosα c j sα) cosα w ρ s sα w ρ c + -------- w w w j ρ ρ j [ c s j cos α c c j sα cosα + s s j sα cosα s c j s α c s j cos α + c c j sα cosα s s j sα cosα + s c j s α s c j cos α s s j sα cosα + c c j sα cosα + c s j s α ] cosαw ρ s sαw ρ c + -------- w w w j ρ ρ j ( s c j cos α s s j sα cosα + c c j sα cosα + c s j s α) cosαw ρ s sαw ρ c + -------- w w w j ρ ρ j [ sα cosα( c c j s s j ) + c s j s α s c j cos α] cosαw ρ s sαw ρ c + -------- sα cosα w w w j ρ ρ j c + j -------- s + αw w w j ρ ρ j c s j -------- cos α w w w j ρ ρ j s c j cosα w ρ s sα w ρ c + -------- sα w w j ρ ρ j c + j (78) (79) (80) (8) (8) (83) cosα w ρ s -------- w w j ρ ρ j c s j sα w (84) ρ c -------- w w j ρ ρ j c + j From (84) we ca obta the result for α or taα respectvely. + -------- w w w j ρ ρ j [ c cosα( s j cosα c j sα) s sα( s j cosα c j sα) + s cosα( c j cosα + s j sα) + c sα( c j cosα + s j sα) ] w -------- w w w j ρ ρ j c s j ( cos α s α) w w sα --------------- cosα -------- w w w j ρ ρ j c s j w ρ s -------------------------------------------------------------------------------------------- -------- w w w j ρ ρ j c + j w ρ c (85) taα -------- w w w j ρ ρ j cosθ s θ j w ρ sθ -------------------------------------------------------------------------------------------------------------------------- -------- w w w j ρ ρ cos( j θ + θ ) j w ρ cosθ Equato (86) cotas double sums whch may ot be fully evaluated. Due to the symmetry of trgoometrc fuctos the correspodg off-dagoal elemets ca be added ad thus smplfes calculato. For the fal result (87), the four-quadrat arc taget has bee take. Ths soluto, (7) ad (87), geerates sometmes ( α, r) -pars wth egatve r values. They must be detected order to chage the sg of r ad to add π to the correspodg α. All α-values le the the terval π < α 3π. (86) -------- w α --ata w w j ρ ρ s( j θ + θ ) j -------- w < j w w + ( j)w ρ sθ --------------------------------------------------------------------------------------------------------------------------------------------------------------- -------- w w w j ρ ρ cos( j θ + θ ) j -------- ( w < j w w + j)w ρ cosθ (87) 6
Appedx B: Dervg the Covarace Matrx for r ad α The model parameter α ad r are just half the battle. Besdes these estmates of the mea posto of the le we would also lke to have a measure for ts ucertaty. Accordg to the error propagato law (), a approxmato of the output ucertaty, represeted by C AR, s subsequetly determed. At the put sde, mutually depedet ucertates radal drecto oly are assumed. The put radom vector X T [ P T Q T ] cossts of the radom vector P [ P, P,, P ] T [ Q, Q,, Q ] T deotg the varables of the measured rad, ad the radom vector Q holdg the correspodg agular varates. The put covarace matrx s therefore of the form C X C C P 0 dag( σ ρ ) 0 X 0 C Q 0 0. (88) We represet both output radom varables A, R by ther frst-order Taylor seres expaso about the mea µ T T T [ µ ρ µ θ ]. The vector µ has dmeso ad s composed of the two mea vectors µ ρ [ µ ρ, µ ρ,, µ ρ ] T ad µ θ [ µ θ, µ θ,, µ θ ] T. A αµ ( ) + [ P P µ ρ ] + X µ R r ( µ ) + [ P P µ ρ ] + X µ Q Q Q X µ [ µ θ ] Q X µ [ µ θ ] (89) (90) The relatoshps r (). ad α. () correspod to the results of Appedx A, equatos (7) ad (87). Referrg to the dscusso of chapter., we do ot kow µ advace ad ts best avalable guess s the actual value of the measuremets vector µ. It has bee show that uder the assumpto of depedece of P ad Q, the followg holds σ A σ R σ P ρ + σ P ρ + σ Q θ σ Q θ (9) (9) uder the abovemetoed assumpto of egl- We further wat to kow the covarace gble agular ucertates: σ AR COV[ A, R] E[ A R] E[ A]E[ R] (93) E α + [ P µ (94) P ρ ] r + P [ P µ ρ ] αr E αr + r [ P µ (95) P ρ ] + α P [ P µ ρ ] + [ P µ P ρ ] P [ P µ ρ ] αr P E[ αr] + re P µ ρ + αe P P P P µ ρ 7
+ E [ P P P µ ρ ][ P j µ ρ ] j αr (96) αr r P ( α)e P + [ ] r P E [ µ ρ ] + α P E [ P ] α P ()E r [ µ ρ ] + E j [ P P P µ ρ ][ P j µ ρ ] + j [ P P P µ ρ ] αr (97) E ( P (98) P P P j P µ P P j j µ P + µ P µ P ) + E j [ P P P µ ρ ] j E[ P (99) P P P j P µ P P j j µ P + µ P µ P ] + j E[ ( P P P µ ρ ) ] j E[ P (00) P P P j P µ P P j j µ P + µ P µ P ] + j σ P P ρ P j P j Sce ad are depedet, the expected value of the bracketed expresso dsappears. Hece σ AR σ P P ρ (0) If, however, the put ucertaty model provdes o-eglgble agular varaces, t s easy to show that uder the depedecy assumpto of P ad Q the expresso keepg track of Q ca be smply added to yeld σ AR σ P P ρ + σ Q Q θ. (0) As demostrated chapter 3, the results (9), (9) ad (0) ca also be obtaed the more compact but less tutve form of equato (). Let F PQ P Q F P F Q P Q P P P P P P Q Q Q Q Q Q (03) the composed p Jacoba matrx cotag all partal dervates of the model parameters wth respect to the put radom varables about the guess µ. The, the sought covarace matrx ca be rewrtte as C AR C AR T F PQ C X F PQ. (04) Uder the codtos whch lead to equato (0), the rght had sde ca be decomposed yeldg T T C AR F P C P F P + F Q C Q F Q (05) 8
B. Practcal Cosderatos We determe mplemetable expressos for the elemets of covarace matrx C AR. Uder the assumpto of eglgble agular ucertates, cocrete expressos of σ A, σ R ad σ AR wll be derved. We must furthermore keep md that our problem mght be a real tme problem, requrg a effcet mplemetato. Expresso are therefore sought whch mmze the umer of floatg pot operatos, e.g. by reuse of already computed subexpressos. Although calculatg wth weghts does ot add much dffculty, we wll omt them ths chapter. For the sake of brevty we troduce the followg otato wth N α --ata D --- N (06) -- P, (07) P j cosq s Q j P sq D -- P. (08) P cos( j Q + Q ) j P cosq We wll use the result that the parameters α (equato (87)) ad r (equato (7)) ca also be wrtte Cartesa form, where x ρ cosθ ad y ρ sθ : α, (09) -- ( y y )( x x ) ata -------------------------------------------------------- ( y y ) ( x x ) r xcosα + y sα. (0) They use the meas x x y y. () From equatos (9), (9) ad (0) we see that the covarace matrx s defed whe both partal dervates of the parameters wth respect to P are gve. Let us start wth the dervate of α. D N D N N D N D P -- --------------------------- P P P + N D -------------------------------- D -- P -------------------------------- D + N The partal dervates of the umerator ad the deomator wth respect to obtaed as follows: P () ca be N P -- P (3) P j P k cosq j sq k { P P j sq j } P sq + -- P { P c s + P c P s + P c P 3 s 3 + P c P 4 s 4 + + P c P s + P c s + P c P 3 s 3 + P c P 4 s 4 + + P 3 c 3 P s + P 3 c 3 P s + P 3c3 s 3 + P 3 c 3 P 4 s 4 + } (4) See [ARRAS97] for the results wth weghts. Performace comparso results of three dfferet ways to determe α ad r are also brefly gve. 9
-- P { P j cosq j P sq } + P P Q s { Q j j P s Q (5) -- ( sq P j cosq j + cosq P j sq j ) P sq (6) -- ( sq x + cosq y) P sq (7) ( xsq + ycosq P sq ) (8) The mea values x ad y are those of equato (). D -- P (9) P P j P k cos( Q + Q j ) { P P j cosq j } P cosq + -- { P P c + + P P c + + P P 3 c + + 3 P P 4 c + + 4 + P P c + + P c + + P P 3 c + + 3 P P 4 c + + 4 + P 3 P c + 3 + P 3 P c + 3 + P 3c3 + + 3 P 3 P 4 c + 3 + 4 } (0) -- { P P j P cos( Q + Q j )} + { P P P j cos( Q j + Q )} P cosq () -- { P P j P cos( Q + Q j )} P cosq () -- P j cos( Q + Q j ) P cosq (3) -- P j cosq cosq j -- P j sq sq j P cosq (4) -- cosq P j cosq j -- sq P j sq j P cosq (5) -- cosq x -- sq y P cosq (6) ( xcosq ysq P cosq ) (7) Substtutg to equato () gves D N N D P -- P -------------------------------- (8) P D + N ( xcosq -- ysq P cosq )N ( xsq + ycosq P sq )D --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- (9) D + N N( xcosq ysq P cosq ) D( xsq + ycosq P sq ) -------------------------------------------------------------------------------------------------------------------------------------------------------------------------- (30) D + N It remas the dervate of r : P -- P (3) P j cos( Q j α) -- (3) { P P j cos( Q j α) } 0
-- cos( Q j α) { P P j } + -- P j { cos( Q P j α) } (33) -- cos( Q α) + -- P j s( Q j α) P (34) -- cos( Q α) + -- P P j s( Q j α) (35) -- cos( Q α) + P (36) -- cos( Q α) + ( y cosα x sα) P (37) Note that we made use of the already kow expresso P. Let us summarze the results. The sought covarace matrx s σ C A σ AR AR σ AR σ R (38) wth elemets σ A --------------------------- ( D + N ) [ N( xcosθ y sθ ρ cosθ ) D( xsθ + y cosθ ρ sθ )] σ ρ (39) σ R -- cos( θ α) + ( y cosα x sα) σρ P (40) σρ P P (4) σ AR All elemets of C AR are of complexty O ( ) ad allow extesve reuse of already computed expressos. Ths makes them sutable for fast calculato uder real tme codtos ad lmted computg power.