From 8f2110d95816c577b9dd6faa3f9a2c371fe47cc4 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 1 Sep 2024 09:51:35 +0900 Subject: [PATCH 1/7] add turning radius calculation doc --- docs/modules/appendix/appendix_main.rst | 1 + .../turning_radius_calc1.png | Bin 0 -> 14926 bytes .../turning_radius_calc2.png | Bin 0 -> 21792 bytes .../appendix/steering_motion_model_main.rst | 342 ++++++++++++++++++ 4 files changed, 343 insertions(+) create mode 100644 docs/modules/appendix/steering_motion_model/turning_radius_calc1.png create mode 100644 docs/modules/appendix/steering_motion_model/turning_radius_calc2.png create mode 100644 docs/modules/appendix/steering_motion_model_main.rst diff --git a/docs/modules/appendix/appendix_main.rst b/docs/modules/appendix/appendix_main.rst index 8a29d84676..cb1ac04066 100644 --- a/docs/modules/appendix/appendix_main.rst +++ b/docs/modules/appendix/appendix_main.rst @@ -7,6 +7,7 @@ Appendix :maxdepth: 2 :caption: Contents + steering_motion_model Kalmanfilter_basics Kalmanfilter_basics_2 diff --git a/docs/modules/appendix/steering_motion_model/turning_radius_calc1.png b/docs/modules/appendix/steering_motion_model/turning_radius_calc1.png new file mode 100644 index 0000000000000000000000000000000000000000..3de7ed8797c31c0aff2007f89fe9f23cae4cdc47 GIT binary patch literal 14926 zcmd^`X7kJG<7hP@dcucFW zyZdqH?QNnK1LL&q{rgv;guPnnDOe{Hz6|NhB2_?Qg4g{_jTl(q-_zbrsI z!QRasC5PG{_rww=v+6&2B|XN0W>P(LEVifl`FN~19e+bi2d+8{s;c3lx?`McEBeg69^+w<4W#--LOvBSN}iioa9K>{63k!+x`4- zwWh+^vFrPMq0qFKY|uKMCF%_VKOPmIMGLMwo>a9}tvL2#+QkbtG+WPrZ7<3O>h}e{ zFE_iddk);3t*vBvpS9SZt+t%`y*=fKUQf!bl@@0ocfLNJq*_|`_V>WgfVVfjqJMqX z(>r*)J3r6%+$VNlFmi8!M?~>ZI~)_-aD8X&k$19GXBF*SB#^SkfHSv0{e8~KX|u9v z-8qcXw>g8?>ethLhQsQ*_xV5hXp%hNQ`7u#C{c76qT_JZF2kwb{u&M*P;1)S>;4=x(e`yV7kh zwuA@eBy7qjZ#0>?u&1(mzEY~iI0OFZrT3{syzm`I^o;yh;hiYn)rW(e^LaxTwXWdX ze=FYjr_DRRu9U)H*3F@%iIXtBXxlHZI$O6s(hUbe*pD$5d!Zzm1~{ zn?MHaLn58vr0RwkGWbXxN(+-O(=kZ7DsezRIXjzA<|{K+OvA{*irFRaJtbKyY63Ur zPw8%Dj{kdOf235jGPd{OsWcBIFnULasUBvzEf0;Qa`$er(un0K36;ux(byMw@~ef7 z!??6iyl)vc0oc0w`|b;0m3ReW@2Ks^f?#6;=-@bh8g&%SJFjiQW z6%pTy7(zT@EfBnvftDRh8oYKvZ(2rN_!E%(}FNf|+7D*1Nki>b%UG|(Kp$BE`dHu99>jOzM z$mh}H5Dy0-UYP+L?`2`@Vf@ASq0rCY_hICV2uQMuCOAreAMVDX?O$WYzz}FJQA^D7 zRt8&q*Hl>_)LQHPDc;oL;d?dxN@e<_)|}gR}^Hqj6;#fj~)1TJgN)Ae+L=ZGha}b%d=>b!Lq4%^vF@vabCW zW!Kc>VJ~IPU7E2%&!MVirl{rd?RlV~(Bryp!f>Kt!ENM7%EaIaTf&tQ{MPYKUKdS9#T zC6<@cv|gkghlMI5wR`Ptv&HvQ&i-K>8yPwOF!4Aro!>X5@%$ar7M!j&muF|rLkM1s ze5}ZwFna`-Ppm8-fL?r5DEYE%9&@cK3a~#YbzVU!!O$N2mc8UFOQOIaLE-RFiKnHB zbX0`>u!oDof|y6kVE;I^9MGDbf%OMq%EsI4ojvUFk9e_Z(l!=|JH7JHf9n0-xMXRc z^<)0xZQ#!cS-c`JJK=-K(96k}H``1K1XL}Iw{WpK?os3uQ|nKj7J|m%!F~zC z*ZD|ud};bP*f)`QY+saJP?51cCW25fFN^S3(h#w=LGaM4oVQic3eTB>Qn(#73|7ic zhpv~~^{_-?{@1tXtBI#fVhZzodlTguc0@naz=Kdyd&f+Pi0f6C3I24$`_@kfe+DQ{ z+%|mNn`%9>33#>=5?SK{J)fU8-rO8Y+YZ~RJN7||sM?J1b)`uCRbI`4xXPwk!4SM@ z7D$m`08$@<4cpb8gU#XeUw*})Lnc?q5<-jvC#D&<&xXv3?!4z4qxIbRbvNy-qmX;j zXSh`s*)Vip)_H{he+0%>0vh%G_N(zLDofzfgv8rw0Hd19L3QUNOZB8ojcK|m7EhQm zUeUfqiqU=uJE(R>U1IHD!-BE>l0{18E*~1nNU*xpx3Yg(`zNO9)ElFU@Ez)UF%{&0 zfA!w<1lx|z#ZIPjz9~(%)MYdsmsOdr{V)=R@0pk>uB^{*!~>}u!b@tQKE+>hN!%`e zJ?jBbUXPnokcH0=S&LAcg4X=rUMSv#pDS1WoXoERQW+7#8png5abwro+$V&Q>uJD- zvZ=7uKpLZmx$jDU2s^DW17p^${Gll;N_n*$&cgD|F7Ah5`bM%+$XEs_(wT!Z!DX7QdE%65z z{+O(MHv*s%BYnuSzjccQ5=w839D#SD1s?}>zC<@{_&y24?Q+{e%QthnnIErb)V%@B zzs&r1am7xmyE;jQoz3${P#i7aZmRumGz*my7}%$WiT$QtpU%(iJ0C0oslJs7s5vAZ zh#n=}m=vD7xPmunR)&Hh9q#cz+nIkcO>(3V$`G6!Mu}cmYnZMc{}3-Mh!095W8Qh=ri7CpAd}+% z737%udNbkYHc^UxXy$x<#zE~;vp@Ga0TppTD1vks$C#@4M6B@GJ#nYDLp4tzQZh-J z@44-$DC0ROTvuiY2|`C2;!Ejl2TI66HI>uq!Q?*TQxDee1nM=q^gobZ{Us<66tB!; z_RBS}6QDVYKMC>+ztIy->ur(NO9jM+dSHt|{qOAhsKNUxb!h@_6&ci4yErx=@rGNC zhn#A*9j^z}NypWywpM{hwyIcXJ0PY>MZb)9&)tO;YDe)tL?x|*j76rN7? zvFJDQ3!D6KW%yQ-kXqe0d=|dL9Ex$=%8%i#(b7xbZw$Qmb|i0h!Av7_|2mrNSOMqT z&b#|RRryJOhfq2;D%kQfYu4H9)W-j6rV$d2GMNaZO`*LDWJ1R+?_!Cc)qM>?Z(1JyUQ`>3 ze0eg<{0(bN*YEn#_r*~32f{zuL}gT8O+1HFiPwu5B8%U%_x+Wf7f5se(b62Mzd z#7B8AYmD2;UK=-+44b|EU1$}x4Jz=G=?hMXagg(w6@*dxY@2I0v`0zV%uoQbJB$n; z_x&x|m)2FSXHQWsyi-TkLL!OhdQQp+5@|{QIySFH&a_o=){VF}9jQGhN477wvWkT4^1R64;B^NCnB=UVvAo-W`20Azl8b6 z!FpH$SJNt-4-gq=Jz!J*L#!?K#Xcn9yO%`X`ADL1j7@|LF}%|deM)oXZznl9Yw-p%H;cNiCIgzzxbI#x8ZCP-lH%*Bwhie%$ zTi$1yt$l?x?Mk%aI-~obub)nN^U3@u9)f);Th*SFJgzn9MpglIrl>6vE%Hq3J z5okUC_=(wo+Q=)=ipKP}lSV#|vgLgpPcOFUDc3nK0xs2`--3~vePprBc#2P`UUz5Y zgXcRF92<&M3TDuG0|!E}^yw?aBvPcC4PVk{E;CuKa&PyJw?d>tURZn(nD+7nm>Gyh zCmK!Df(olFX{IaWk|jzCGxi7{Wa1w(aPINZlNa!XY#x_aV+9XsOb0GM2>alxJbeq}XDd zwuE^Ej_egHZC_Tm0LtAc=4|7g3T2LV_#ERUKer`hh8m6cGDNqo(E>b8$EB5T9;7_HjQ zww~D4C@;Q*1wIJZh=!%6x_KL|njXi8Nq&u9)A4XJ@60EO9zzecuPPm%aQ`ae8t9e( zaxu{MH_2=xetvej(ErDCQ`gH_0m^otY&ek_L!;X&cwon|{}kq~#i;ysC^!M=TVz7% z7{HNT_G7=gW%Duv4blB;!A3fD9nNo)@{BM*$t5UMVeLCDsy=c#^%0eIG};l?c4}TK2f!A zNIp>a{FD3(NSkW#|c_H%2bx z6hi}eY7pmekiqISarRVPe&}mko6imChQ#SrNtO@fwwY zP${!Lm3Qe$4J7oHKMx^Yp?)VqzheKCPy z%wr5(3`)?dRZp1!D?f3sx!&E$0;h(OLgHzS3;&jX=0kvUD?$T1S}^yzxr%33`o0!K zG-wc=@T9ubg^>&I`e|IgU1_)P2Pg9ultr%A&5SXEg5;S0u;~Mu)xk`1ZC2glM!2yi zP1##l1lVX7g##*l&`eLD%qbW*67X5^eb(zI?S&MBMY#@! z{hx3&BTFT%LK=ETBg}8MhaIx4UwGn&5Q?BUgWXH~R5e-^@=^TY=s8>(n*$KIlq0(S3$e`|PinVv41^e{mU{fER0xRz-W8=Ah!HE#8F)GS}%YnmXKv(8F zs-pA)bru02l-?l#*~3$$OJVB~d0dl;rQPg{5R3m=e~0I$`-bz#CIvmVnVG3Azv~In z=um>=qK6}EC)M-rGddC;;ot*#F5Wv8y3>E1PiJzdm1YCs&VJA0R+&!nQt3vCJIgJ# zE|R6B>&O9qam#HVOWX5On8dcZbAlNB>%-tPkKP?yV8Ls8-o`1k(k7m}gww$Pu>Hzw zzDwch>FH7ghAbz-(O@UZu{Qi5`zW?=^!7n+{Yk0e^Q9M^nG!W#P4 zp#;!edMah1@NNw51C3LIIg5_%9K?xWeQQncZcx(1)OW}h?ftWJ-B{pC;Zg2CXxg8v z@gCTOM*8vU+0Z^+{L>(T&%4+(?_bbD%bPOw)qIR|mEP%F(2Yz^xo()dd{(_0qHY{2 zDg1{?k5}e-v5Z18AvZ1dsInuZI&eXeT{ybcIQ()%_q(SAwKCWlIA<>WgC6Q{QC~iV zd1AmZGm{~$iPs%jNH|+;1R1$=2N<#p^(`~A@k{=p`%;LvuWK`;Vx20ON>O1cgRdM- zp>iLfkzeL59!q%jcYIB+Qn8d0Hc&<4?)E0#vI_FQ4@w8yyQSJdGIwYcW?cyq85RINSuf)9K zd|Bpr&t^{$;u;rn4B=n>pW=X}6+Sk)>LI2jGPr!5knZ*zaj;6K7}Yc2&t%8l`iYAb z@aq+v|IY+=m*)6{-YDugwEj}5Y)Y^3+0vdE0QoL5wnlLsy~jSssWi#lDh`Qx?4^GI z+ozIA)B!6$xgPMm&AmZYm$_h$@8eNV0D$7X*z5gHi<`>q`7Xm$^i=|n)lWv-DdsE; zPM!vXOL8sGUyiV+Gv1oM7>wOX?q6;pf@H1|&-Tup@9S_PbAX7t2WFJO->bgCBcG>N zI*;E;vXvLP4T^F%py+wY5IkH&Bv6VUP?W4EbW*d&22RXIA6OaddB94pG@@^7n!Rd} zNJ9<2C-Zy0_>8Ra4HEWK5|}ikSuYP1rOVW@ebUzi&|(>oHKiH=gC&cGMneJ>5-|1$ zTf`)!!x#fm{zVERk|+uQz=JW;Z)OZ;jTRy?Q%rMVtnc}(HEi+FGpdNA6YUC;$JG29 z&r$c`hQh2b^WY$mp;>=3d=Pb2Xr1WF^}st{ zPNDVqzDSOt(XiSGANcGc0-sUf%A%hJ>?rWn7XG|@CH>n@@RYVUgQTHz-Zn+r+ zLe4RQxO{!NHTv5JsP^WjInyoJG2Re+ zp2{=}E=8i{tnr~fP@V>*My(3UAML>1nG2K&#pf|>QHu{9X)1M2Xi~2vgGny^2&SN( z?B!LhWeR9)?xfh4Kdl1_)(xtD@`R|rL)eO+#y?rmADLs`D4Gc+J@zY{*&RyAf@=qJ zL@U#Y#hV;i**z#a`<1L9G^MSk+EP1a4h8TiQv#Y9>oos{U?=1%CWLh`ha5hjk%|iE z_^|l2BnBK^L*Tw-0N#affx<5c8=OwOT*gw&mdFr+-m z0{L0wDqWP4AbjMM$;xdnJOHOmxcaT&cuSIvcswnzm|3* zSN#hgn2ln=Hqmy!9WS*$b&Aw<{9nq8@gALXm7vsy!Nc7C5$hexC-xD%o)meg2C1$+ zwQ(}Cc_!be^Z5PJYlaz?w&6VigjGbE$|HDZTa<~&S@q8oQ~N*r_6zxiVm(gxC)@It z?>_<`{w=pWFy@>lAI}so&OE#*u@{p``_yDf|NL-c`l~8-37A*F)waR7ay$PjFG?xb znpxwWnIBduZge`p%5psG&wcB;h?KttXCeM4)reg!N3fB`WSxv9Y$6OmN&V;2d8EaZ zCq&@w)}Z**Lhwni%b9%EUVf}nKYg2KWr{5rx*tLxh@XeD{{bZ3{83Big9M6i9c_qc z@I$eN74JXRjpz7A%!D~u7PCs(<|+L|t!p4agh9dz$zsvw^l`Siap4wV2nZE-~aZ>o`?@j|g->DJhQhQ;~R2u98zp zlol-M!lx_&Xg+`qkfmBzZg{H0lXv%rt@3dGVY(_gu4R_WZvr7?Hie1i_lHD`f2eIY zWDTHox0UZ%!#vI(NH=2q8?(H8J)^N0o46u6mIy@_T`0(UP)0vN&VTb_(F z*PrA|(miu@bcy2n`OH1S+0`z%036a&#=&8yxdX)V@bTB@@o5%+$a;3=HdPY(&s?_B zSDF!BcS9A_yf&#-#7#~ed~pK9W%dgQc+|^S;Xmn4D?1;LKV^MTj>30KH2E2O&jRplU^sQtJ z;uB9L!IBp)sBd^@j8`N8u&~M}$VL7=DO!%9kOJaswSY2$Nlm$K zTMCLD=R-_r%JN@;dvFiUo$(?b;C}^{-Pugw11dXimp?QQf&!anK8OqTZulMer6zX8W!K@z_4%TzN{NTc zH}$7Z&$yZzwkrk18ON8>c8KYkPFuAkYmyt=FSGA{6O-BlSxevPO^e%3;l8^r*>($B zr^6HYX_VyKhMcelKLnt*9k0S+6J=kK|0BCx-O7h(Q+qCGLke3wxHKq&zWWC!oCi>E z)GYELi}tw~?<)bX(KGe#tBG&iLp!VhLFD2gcPDpG@Nq-u=+IOyO~WEcCj?3z5!2*8 zQ)BCAR8sob&iO@Zp%=>~3+XV#Md$rMr&)GM1eF>&)|RqDX1pslYSXfn%?{20yqb_` zL@<B7fU;2UuFz%k>6U9y5kRY zgfViry&zVU|1&{gd~mY)6-}le{ad~!l|MwkVLXdlH$g6Mf-z-Zrcnr2`IZ%cqFvjp zNnU4DASnM>hEWIg9Zlu#+)Rk|)XJ|20HPAt94GDR+*u@UVT_id<`rc+cV1GwM!)B# ziNofSeA_jpri-4Q2q@!fYsRp>J#f+E;T(E8R+Mt4JibT77TfHPp*(*P3r>$F+Jmd9 zvJMm4PEeF>8$=TvIr{g7^0cId*PX&!OMifHcSDMfMmKR&1P(mS{p4uhjj%NFg8{Kmc-<{}5 z_N*cm-Srl;W!MUcR=e?z4_{{I>Bm$ADd`nnxBPb#DYR2$!GrDq#H{q0r^9!`tUy{S z)gd1F?|OFT=QY2Bx1D=zC;1y}5-VJFbtmGWp~}9>YVicE^3d4m56R7%kHWEDu*yzo zJlgEVn83;?47;gb(t+29&+Cgurw$)HmAi+Mnp)Q9(TQ7+5ovBwGKm$+pH;P;l|ctv z!q7uvY(OoZiAC(C??7)46MpS)p+urUT(D@GtTlOF;?lGw`^{HM9`kUtwJ;G!zhC0F zpYC8Kf~5={3|TueKI_=?uM!r+iK@x{mwv(6o?%VpbM9zzO@o8A9a z zJdR%fo*SCG^!a$atX2I`<0vAfL8fGv{t>kY=?shhed-s2(rF-91ogqkg-@qW*2MKb z4AG{rL-sEP#l!0sdLI}RS0|8+uNwl5h<9oK=cMs9QY}MZEW@mx^aJ|#m&Eo>L+DQG z*IyTT?yYtGO4i1y3k=}xcC@MJ?+``+r=T`|Pu<+ii7H`Xpa^gslPOGT#Ak^6MbU5F zX@I=LO{qyQbJ7VH5*A)4oVnAAF!jq0!iX*5)H+QNs{!&^9KuHk4H_?QpbPdmyeQsE zEcW|s80_~;j1C{_Ly%tBW|1QhgJ|w)uF?AJX2j?lL*>pVk>ElWVC5TmI2(6^<1d3wva5Rk?=oY*U`RSjVv9TrcWc zu{vc;u3~vs$`@bZU!~fvs-qg0`TmVm1?|&)KSPCgA=jnX;BW2oPEt8Aw`7d13S99_eF{94uR743+LG$q{E?Z*k^e)?1 zrj+PS#c#iqqoLHpxVoRTP~tw*rTVRjJPvWIb^2~g7UCeh@1>}4yRTcBPVot;sPX;WT-}N1aq{>t)6xB!kqgWfGhGJ1|M;eILtHOi0!8Dft@aH8p>xwEj( zn|d@Fr=EjM2?2SumC+2l(rM0G|8wj2u7P`rid!x6@DO71$wq z?d6SADtnUoArsd}Gixxc^LWF#{Q{EFGW(a|MTnyHKYH>B#H3A67X39jEK2$8H@-BA zhVLi?4xj`P-KHQ~;F5t*lXbN8<#uJFm>OqD+Iq?qscWL`ymRF~rPDNlLG5zE2Nr$Y zRK{`5bUXEbqhFe3>L62eEy>D=(q|_7lge-pZyWLH4K~uLs56=Cq*O$S1#9pd>}~6s z7^AKRs1-Bk5pOL}D*&ZaH~r-uFGR7dk?ge~qiA<|Gi(gD8=XGl?MgE*r{1kXvDP{2 zpG5msa4U1?f#3ZnVj&@}$wJr6@#1PUpf#9Qe1U*%&bI-D+3Ois z$}_~^1f53jaVaN7M0VE;Y^2$bK{sNUOv3;UNxkTDnM%Sal6fb}8%qAs< zXW{d%x6T-IIpGPex~!*OrZqCf3Me%^JKJF2Lu#66ynW+432#+($&tz6&WC+cO2UGY zy29GfZH35vARaQHq&`?zR`V-a9-*l(abX5x1R+S)8%Gi+XX;GgOkg;k&pZo8FCPS+_A8iF;zR4%X@R&&DSJ)gocyq@Kz}{ngI&?eD2H^nH=K>9g{&aRfd{+XhH)r5eFTUUAte0%eUbFTtg12`$#z345}J^( zM&{L69ESluIBWXzcp?efVC)v&*~x$&nchxSrrGU@^fw@$l1&k?`O_a8IH62W&CeFz zz7&i;cKN~B^n{Xf{u>AtC7OFe{>-7wveEBL0^r>8e3I*Iv(r0_+}GxeU?HU3-s@&t zpz6K$*(=k4Vgw$j9X>$0y9n@WrsR?=S33wy2DySItxFdI;Tkzpa1a=fTzhK>-Kbgj z_hg5ndu$0HmAhvNOj}nlZqS2~W>K7Ki}-IZJPvY?Vv$MGq}UHL+5Yeka^&m#`joJ2 zvu~8yC}%yT3~$uIYq;%fACS`Cw!4@58NRaeEh18?!8aQghnC~EBJyI?eLJ2ljjKhj z5z+^0J{ViC)z!(SdOLIHBO0Em8t_$u4S!)wh((L5hhJQo5l4il`JWF5e zhW`fOv<9(ROw*L44@jv^=4wj^^|Z?%<{Q|6AY0!o^*~tkei^7ayI%@qCt{ze_;S!W zn!CgHd$3z!FqJY(hDn}Kwhd`C)kw%AZ5nH1Q#{#8)SCdSAwd`JRIal8Ic_D;R4jzwSuV1YibDSrSDL8OpAfBc)jK1a|+Vktii zrU?hDkg49@$^t3*ax_*xn}!gQ2}{V&4ZsgGg<=qIV~JmDgbBz5nyF-|+K?!P@`n^} zi6dA+A7IS=op?NOOJV3R_-6zK|NsPxtG-G!5@4!s8bg3@E63{ z4h{5%L82rR(OT&#iG(QvwRAte;8BCQ+8~)&>(=sf6Y#coC|G2Nf55QAygi#$q6Fqo zQGAbs%tPssD^NaGOr^oO6YkH-I$lwwzpYou-IzEO4uLg%zosCel>fn=H5L1crb*tvGabU{7fYZ}1I- z4ZDw44^aB8rWl(z6X$+vJx^#X8zQE-!>d_2GLwwPsWh55_WsvgY`!zl4PT!I)9Wn8 zmE6Nz@MiDxjs-!HRoGEX)?kiA+pG@94-?oFlZv`k(8f2OwVw2I8IaaCkSQYlMu{*p z-9=EK>1d9&%|MiuWd2y_NGPO-L)haje^2Ip`Uk{#3R8UC1s=!BV3mHiEOfnzzyUGu zWvR6xkWsaz@t{M4{!@eG)C<{6(^0-aHe?2(X=B7{5qSSl=kH6>fI*s^!{d75WK?DU zk+?0I1@gjP9W$G;AIT61cF~B#!Cuu3s5#fhZRzR45urc|mV>URGf@SP?1_4t8QVzg zx%x%7x%&48DmO0v$;wnrX7*d(ik?DX7Bm6nxEc#)GzdvXqQsIUlO~U^P{R_u#WZeU z4_aJZ{{IwUWiTfJ`k-N`m@?XnB|54nt|vJ1{Rd3+1xKa`%M`VxIUc8Se{IK~;)r(Y z7See6uQv}Ler{ajlhCJ^iY7u50L(71Dc^fYOT_GbpS{JjDX%8SqWf_F9;EVb72G(z zyblwBEO)*(dhJMl`=9I6g-@kp(Wf{Bu$H=ELrH!XiVerGs@298+JuWas)MEMk}UGv zKJNHCQGF&;l#QqE-pnf?wpNWqjs8!Db__FHu=MO`a0E-Z7{8w>_5F<5pH!$fLx zV=#%&oR1G|4gr16FgntVGg@s_NMntRYfEcwVF7lDA{zh?Ul)Ok3{14ieKcS`hKTPy zvDg=d80SdEosL~Ya&5Z|qr}$^rwV94XJ+*YnUE=R0PtXGAm<`N*yh}BmI@l``0~xN zKVVQO_Wf4fFiZLU8B)b5Eb;A{YAXNVjBcE%xFKO@NI375$BG^j`@*K*~ z#(e#B+3T|kg8co}p7cBib3srl+5PNe8M1Qyc4-KNsN0NC%vdG<>bKZR2Bi7#5{v&7 zcabr}j`4odGbt}gq#|p!u?}aEnn!UFQL#08&mFV=#Ejyr$urq!#&i^YOWjM)ajAA~ zm0E1mDUKnDLSWA5(W)w zbhNZwuRJ^qSK;FB{vs#8C%t=k@Z5iR@J&J)#7Oy^MS!xlXC^t18GQp-`;Dcpyp@s? z3LEep69qNI1_d4Xh6;Qrfe#7_S{B;>+k>i}_2mD)t3Lj?OqqI)f+CJ050lXRg1VQE zH3v7p=q~pdD_V$AkPiz7>xYs;h;rWlNMv^3)BinPeQ%j>{~?hdVUbiNZ4=8J%b*|) z;gFlDz42{I+V3_`Nw&7isIN_S70ujAw%T87@VOA#5S`NV$%iwVX@XFt#R*l>d1SKO zx;(d|!u=SsFu`b~s9FdQMlc$%sVa%C8JkETK5qyd2726&|Nn8nKM%G)LyQAD--iWz z7Nr!6``Z&0*AWSfz6U$UO-Z{j6e8 zqzU0Q@4_9gG?#VV`!SV*N5%Jj(f46K&7}{rKBma^S}}=paK3kE;Ft(F=j6D`Q)p`p zshFA};+axu-kkzmH+iAaQy=H7pR4&*?VL-5&1g<=?URvQb~I5&RRx<y(c>lgXC-iOJ^CB9p>btMPdT^)Bu~0m`pw2}Y3Kq6TYz>wjx(G+d`Gladdb zZie6-`jc~CRw9EH+y+x#f2z-9qu&4Relk$rd`~&@_OvEk+y$^UOSfz@2P)dmR5?Fb z++uI$$9f2@4_J7n&Ut0v3BIt&VLl6S)X~|LajK~I^>-{{7N%LvW({;SDRB}TlaY<{ zu*z2<&nJ%91OtbU!c`ey%^{*K#ZiQY3ix68wS>-8HMya&Hel<5gzXO%{V>>pRU zBG(r@4_=FSoN{M+9fwUd$kgcSwW`;o!qoekQi{-F9*LJNATA2qG)1<;CYYXR8)J)I8;M!Qj~~Drq`< z%mN0yS(0A=BLahlhx?mShTW<1%xULtY6rFnU{rPs-WP_vKDS@So4h@%0fVLN&;OdW ze4zEcHTBNxPvWZeRjcW_zd86&TdG@`^9~{4^kH=y^y&_xQ*Q5f(iZpoxQ~4hFwBST z$x=Jns`*c|)0RGqP2LUx_dT(UcsA;na(e649oBj(OXtEMQWyqdNu?@M6U$BF0CZOWBn*DNqO|7(sv4wxvFMcOS> zcs%{nOs`Mvpv37DX}X}tC;TVf8$z#Dq}l6rGRQwkghVR>-V>`oMOx1m*&rH~l74+> z>0Lhg4?5@pGQgCL=+LPQM@NICuTO@>$QYI;*Q()M8tVu8)m~S}c{=4rd`?<*l}&2s zl0kmje8HP$tFx*-gwk^gtWah3%y2d}GGg`Y!^Y+3s%q$MU;Ws|Dhvs!qk(k{dyeY; zTXsKg%Z~*0|BOF+1T0_iRRUFM%6i4TC)(&B{|fr7@K^RauBm04MXQ|2&)+N( z(p97KaU>kl9e%Dd`qG79R09rX!fXjRJD>ePb!-Q*$f}cI)tb2zZDwUA%X zml)q{7v$O-mnd@9cbxZltVHrdUz;9z?vPH)h!Y=IM;6dHmFbv-4w+Nx=I!f|Yd7Rl+D`U!k0&R+7~Bz!GX4JkH#^%J5Nk zN^GNi8VD~2B~v5wU$AORRN*n_J5EDp582o%Z$NjPn-||`u_ZS#BCzq(%%cRlep=i7 zDN4ASYQEPxwevneMWeJoMw##uVG`Tzjb}|Y4nFf~JS|(S_)@9Y{5}@q_uZluZ9K=v zyE^c;;L7orfa^fBPDus!DU!j@`J0M#Zx=R)LPeb!3AF-&<)BX(dM1N^@|T;O-NF@F z)ehaJ7x502E?1n>mBi`e(Zdwh!ZOe|xVq~nwMLB|MRMMC{x9OZ*(2>sLw)I*E96SI;Mq#_psAVGC(^O?*BW`>$By;`vZ|_oVDk42q{w2})`8pATGH&A zz#{K6xD=1&rzL-k850sxa#E&J<@Z``5f^}it~;ymdLH@Udpau1$4ZQZouh~#Hs6K& z5;6;ZIW%cMY6sK$T$=u?qFLlV5eD4vz^E*(LlMq8+dZ)SCd5h@UGol_9~Ry|94qpk zR1Xil(6dS+wVc+38w;svDh1s&)qv|1%=YRd;nZrM^gH6_p!dCzKkQN?&88EGCvSd? zT{;R??)LTHoF5y06xcr)^I6qVGXlR4P&%{Nl2;@OQqJ{CEK|X^R5UJl8zeliQ}%0! zNa3VKx6Ua$+ZQys7ssp^$L0io)$eQY{9CI^Yr6AB`sW!84AC|Avbr|6&5>5k(szG` zO*iX*GGv8oeh8LC=Zi4YWo{avA451IPh&nXFQ!4b61RfgU2;(PBbrAG^~UXIEXk@X zO{FwsQvN!`>G6DCP2*tXx%Zk%P7;gmG2rfTH+lZGz32?{edbdUIfG{tW-0750&c&%{3Uky+D3s| z?!axO`5;YP_{6)!$?jgFnmAakVwj{jUxaj>L}I0W3P1l-qLRPB%6BoN4nKKWn8{7! zMclz5ZtPhyTs{Eu%wG9=R-d4q){NiT7< zh?S_Enuen}MwGypi@H2jVaxr2Oa8M=}Oij3ayiX zj*;@ne)g}|D~gzmFvja2ud%aM=q(OFT)|-#16{pW6>LW&qAb|I>)gq-=n(rc?0wDz zC3RqkTGxP7yucn{yz@@$F!YGK;aq+`SePe8RgHYDQ#N0` z?DOwwQj0umx8 zF&NR^*XgIc=Dm@(StmUTOia{dv2d7gu=0+k$$Q%!mB%ppN&f3_o{2mj<*OdiSC^yw zWc(7pjZO?u5EP`HPT zx-Ek$enEl-+#F)6^ZohR zJpa~Jw?0Td6Yj_w<+J)%v|0!X110zdLOvc^__RJ2(P9#-&KYW=Mj+fHvyC%qk~rL+ z;WY-mTzcY~2=!Hoch&r9&0`f`0V!p)EVNe9c;`aa@2<%ZdJ|wm)VB|0YW#y8L_>)t zk?Z|qqNKe0B-)(Ni(hVAail7>00jXKa`WfA84CVm!e|hWcEPq`9|2wnB z(Ff{m855;y6Oh3Y8eLHDb;*A*!l66IRLE-=JX#(nO zv@qW?*13f;!p;SE74D|u&&Fhii>2J)HM6Z1df%M?n0tD~{ZKKiMq-0iMlfN= zeMrDmw-O~ZV3}lSg~iqNjl$ekA=e~f3l^sw~$Rw?AC?uA5YLTqm$w7xg^E4if#D zr+d2*Rfp3@{W)zTL3EXuD3X=ek2zQP1rLm+dcMcW#3ow0h=>-|K%V<;Z27mv6ws>x zKTOhM1*b~#3oX{u`_OWA6pQbHJazgM)wGf?*!%`t*>Xfr0toyV)u&RIY zT=xSF(hQH4vm{IJhIz1b7#;2w8?4g&X+M2zOXJmHJAUfuu&{X)k@04Am1$!s)dx^q zL}c4^efp}uc946oxxewve0~wn?AA5Ugq;yNBq! z@K<;Pde1b76gX)Soj->}BJULCm+!I6f)|*=2S^>lnd_8omugRUtl1O<6$_y{44uy- zEqNp7$%Q5tmH6i^+{M~eY6I}SJL?Jne|5n(@y2 zMX#6}jr%I0EYY2!OI^tUyQsOW?XdSCsOOqV@{>3gjo{eijLb!6`F1E(AK@BhmD+lj zlboNnJSyr~YDdT>W^p{@**awRXo@8@5$b&Zdymgr>TFv(Okvj4T50y@&Ig)!-NZox+PN}y2h8r7VBDuRO#|I-1A?tmKZUraWe!{ zD1osRGasl@qhOzU}!l&s-Ubk%9#^70g zaV~~~sUMnbu24+K8vtzM?H!a*ARk3XCY9g^>jkhV?OsJE?f^OyIT?S2b3(uOw*feu z75EdMR~JALnj|))rhp02-!k-Y3uVKzqJ-Z5OdS!k)c?P>4!W z_(TYV8uZ~pjeN>5w^Y8icgi9Cf0+dgvTJTG#pF4z411XF*}IF_%LiTB9apXP78f<+ zPFc!>4I~Wk5qzU!G^A|hB%gn7bbL|E)8sD%qp3+{xlKIVo%*t+s00_Tj54PKf6X7O-LW{Y>{Ou zLTei-$25tlA9u!1B%X!j8q<8EK>yEt5$XDSC1pq^x%5VNhV*p04Ipn(9-s z79oQCYMamDUkREb-!r-li(KfA%>mE{6X(g8t&Ok>c|cZE#;4)q++IUGEsp7$N!Ik${Xm=9|iz=$X3QAm^&ew%gz%aps9+$rLGL zp1~Zsuez}Zt9e?G02C5&qODIsJ%3}3knQ`7&%#WX9k#lw^YQiG#Uhk1wH zCf(EIl=rvc5RDi0tqx6=JooZ?83IJAP6*yLz7_lvjIp=FggU{D5>4Fryw=?;#i+9C zYcG!9N=yG;^mCJOVv0$wb{(4Rm|);IR{h5zD(eMP{fE_hB-X?n-Ylb=iX8+goQq?& zVvO7f^3E@*5J-sEW;E(05+J)*(Y@5Kd2d>(+MvQh^e&*rX1uGaK-Q)ZSLDT2q>C$S zN{Y`uJ=&6Qy_?AwRlz?*#B`!gcV|9MvX`Gp;7pi@r#gh_bo8=&ZQB zDV*w%ZU44rH&`&TMGwUGb#qGjWBgoMr;<|h1uAM^M6U`7S|!p>0T8&z$^U&6%^jpT`{FbJHplX9ju_bVo(a7K<@ufyOHE4n7Qq zMrJ7L(=nl-r`>VbyhgW@ta6^SFLT6>LXO`tf;>`xs;@ZkCKLIH`zi9Ezo3UZA(&q8 zkG7~^3r{bpu@r)rI7my8%KE_L%(({rld52CRLto#C69{ACP&YT(9GFv7C#+&{)hn1+=lA)Tl7 z9bZ0;+lDI<23vV2wSBPNx~d3|V3{NnA=CY|H}j|AMOP%1&S^1x0SEFU+Zfb)TdZBG zV3SG5XJ&(c70%+1;qT+IVewQJ>tnf5qdl{xhv&x9WT&v3!s!om`81%o>Mvvs>Z*ym zyxPc2Z@gHTCF~@5<6jO2rPgppwx-20%Fei#CG;RJB_66^!RVhGWB!$K#XZwv;QqM} zRcQ0iPJ`zGy@oeT!{#sZ7{(eWtVp2-b=(Pq)a#cGi1+d>y;}f~&&coFx1q3wuD1}% zi5~ZBM_jIy$PxX$IYTA7J>CY;yQyv7bBg{uJNhr=L{sJ)xv49!JjZf3R9ki~oL&Wk z(A6oiPcqv2LqL=F&CG|X!OVl(X=H>}j{fSpF1vzT;B7@U1T4-bt4hi}nO`J6|FWVH zc0xTz*omd3q+{S5?0VeGD)gcG?vrzB$g-!4@s--pNu2@|6d!p>_XVto^FdZy@%(d; zr-HUHVW!r4`-MU5G7fT!)USVRnkCUXjyU-WR6Fl_zB?V|hVmn^a)LxCbu~|S`p3?q zf@Vply?%~vd=+k$XiHqpI9++dvrl2Sg_otOV_;Q!Ug!7!2uWJsS34*%FAF^cLWp7p}u>!FDQwr{w4Zffsa&5%N2zIovu_TzM=;_wZu35 zkA6Y{?wB5{KXOnH_fyuZN)LB4bBZpUCK9x`Whw*0x@TrjWHlw^Gd*PcZW-c(jNST5 zKy`eQ8EU8I-n3@uaue3UYit_Y8CUOv#be9pR;L~ynlvAX%R@{g=I+A zyIgZ}sAv+ydXz!>LahHPNukLqSjVU`-*~<<7s3pYLo&bojgV{!YLaJ~m?|@zmE)JC zvS5_`fk0~~wo!6)ESMYM+kmgyq&*N9!me-gkO4#c zT$Kb=+yk2?gbRh=lFZ36Gu8%z5}!*m7`}0!rN0#ERH&|xU+%Bo6Yj{gb_F-Sh=y+A zskz9-*l8DA`NRfY6cKf@Oy3XlIlT3E3QHsjHdd!!jJHRrtlPUP;O?`^eF=Y=pu)fK zB{%l|l<(kdL<*lo^8P+AWq`d;avk=a7boXisgC*Q1c(Kzb*A@E`Y^GBj()8t{w#?n zstuJ0WQO72h!>TDX%~yWi?LSDLp18OoZ2}X?nrc=_wwwkt;(~LrRnU5PeyfrON~#5 z*9X)8@LDI5V<)~cBbhnxR&9G}Tpnb}{rXZ}eCh3J-Tq>+K!*^gBi24u!X|x-NOJjd zvNW6D%ulk_Z|divB%LmBFQsEeW*)l}7IzQRY^!mzh>KTDz6n43bUS}^n*ElX!*AbIx(uTps;UQhIxS{@RMy+ z%FmO>xRX~JJBz?Ct1dizwll_3Ows4wBi%2O>s;GLwL7jdp<-$gA~rF@Q}E+v*IWUM zUaFJmR6FMbF*R)KmBN_ty;jE$Z$Iy@_3?)L#c$)e} zu#;^xr=@N&8>W_Lgf&UI`&zf<%<}DL#SIlW7%`BFQ1Q;g+pegyly~r!B;d*|U|#+m zNra5bP|iI~y^yR}SIe2$_Pi56HoL9tb}LRb6PtQqM>Caa;gwG5Lzw=P3B$%mTFR(c zX1dkJ!WmCCbE8v~iQZDaD^3*9d|pximZJJ9C*tjza{8D^+I+_G8wEkEEcu3zSakPs z05PfSmh^A?Im~B-$&rIKfpt-k%xxM#mVbEm-QqKWxJEA?de2O@JY%a4n8;O2*l>mn zgEA40p0*n|i$e=hzzx>%4xt`Us+wEyy>emHQncDD%ulHr6IM=XCgbn$^@{Gi$gZK6 zI65H^muM$Vc>6_Fb!8v9h%a`c*OnR{@h>az`Aip^GG9}LcZfcW#O80Ag4IgwQv6-% z((=khz)oQB#e8&*7TN>z}F_&<7`auEQVgneu0Jm z>nTZ!Er-{%dE$zzb|Pc z_L!M^@1wwIvgDl(g?YNO%1XUD32+)&@&j-GYu|^POb*(!>RrB6;+x}suD#1n=D5N3 z?MeVXF?fB>&(t^W;wUGGh_@5S7qE_9j%TXi)d=9y=}!-ac)H!EZYf?G^m}-GR;qu6yxqLKLD&aJurVo`PBi zTXTBQ3f>MGLd-OFUGkZv?#pvegIh<0P>BQckVtP)l*_t7T$%;~J+L7EfI+8lc#{2x zOb@bOv58r6cR2`o^qMF%u8%!+4E9oj%{@EQ_r6}qIftvxscZ%<2xyd#HlTxNPZbLZ z&sb_fxH+5AW>}$1ENZ>f=-k|nK+vMni5aTb;wn4S|T7*A6oi4bD-5+u&5iop;sQnHDtoG26`s z@H92VSW`Q0>_n!cOTvtOqdHZvX;IunE-Mi|N#&Mgo3Bm$I>cD16FXdl#>N0aqt*!% zU?VMM*DXRwBA2zDE_%C+6EDY9hgl?X)V>X_^mjQoiawjh0FT3NQ4uo#KE?ig&1V_q zGWOG9)x0@?TE>D*qY3I|&Rk4dnOiiEC=c4U;bgRl)bJ&v>8ateQOy4&fq+ zh-5}ofRh1Mh!p}hZ%zxec|Z9=^?g=#3v{&zYE-e-5<41ZZ;p~cP_{D6Cc>bIfV%Kk zYS^Gj5k=p*H@Z1vWCJdh*!{YoynOQkHT0i9g zR!N{qYjsFM*>pp5Wuy7Hejf%Ht}m+~w;A-?T;MIwn}pz`(A!IcLcf5u@Wf#Wxx4GL z$WsZPIAhy)wT|g7C>)`cF3M145MZEJQv(}e`l#F9&=b;o zhn%va(SfVzf=}9$`lMjkPrNRs@_0XkAm2&4mHdonhTn#|t!K^VMWJv~?)L(HzTq*| zW$gBk#IM8B7&_Y+X$MQY%jiRm*~?RoX99v56iQ0K-00H%JVq&6R_Flxr&WuVW(;tD#U5aS>L0FIP@}eAQEQyM{OxzNO!i1S)a;e*KR9j zH9&qR!E%|PCW^g

6Z`_+{8?1B@H%YdP%(7_CGmCl6K+id9EI=PO@jD>J48ybd&T zqu(J_QGVgve^T3#I(NJP0v)J)si*}!13y&JtEd8Oi5DLeGZ-zOx)i)sw5c0MPXLit zh?)kd4GwGVon*%LQKVg-IhW{kswGb_p4U9LOZ;jrrwo}x!2KnEJ(J*-AWdi{M4=)t z2Pffa2ofs5*=o2qe%(jrG-;qSSj0;5Ny4I;7XkhR*%Sp{Iwf`@ExjZtCY%NY$&9@Y zOrT^z5}qo_Ej=F!0GMx2Mc{2Gzrr;+#w6uVm3jz6zezW|A7$7%KsfMX6B@rSxm65J z3p?-pDW8|g_lLM9Fu3*<;P_PkXtX^dw;LR%35LD5IVZ!hp?i>xjeC{=hKR*TY{@^7 zK*_}zqjhO-Hz0kHEIbB~vIZx^RxmnilR|yt=rn(`u|U;9(6G5%aZ@QtXH~wG1NXeK zt%+k#v!fMGEJ3k&i(7or%NA|KnycOix)gR8>)L09x2wC=!$0dg(*-|bb7ad~K-J5* zs0HX3=-l}=BzoJZ6eu7nLYS&FzPHZBay%kd6Bq5*LThjh`~7u*+V|qpZ7w9cj2x{Q zE5lo7t773764Sm zb+NINdp3Gu|3Ml@*x!@(ME$L(ss84XB-68KpcVTp>$k8@m3gjQ?{CbVXqM@}XTNqC z#C41UdNB8Csy?-WV~=7zuxP{#yejr)Yis*4p&GaO!dT9X6{mw3rt*g6Mk4LES2!#Z zn0n}snMbFwqE}!Y+CKr-3#LQ}9{$##Ov#hy&qVQo-0T%9W4lXLZN*5;96rt>&@GkANS+tkO-f~{`QNT%>i@Yr~n^3b1g5IFeU7$oh4fffgVtxo1=(W zwBcsDA`WWUt?Dna45Ce7Wk^&P#cX|}uB5w+L9a`_&4`C~<&;6pkAJmv%efU!)wO5} zxoUl58ienQ*+F?*ut^$rz=QcOCmUw$vBm6(+G~oueKqxzQs`I_QhSzq`}E-z=}T=J z0MKxuwoNz2#;g!b7I4j@XB9Sj?v5RD=oUZn4zc=*NGXJb)+H`0Xw}J!M^p9(fEnKJ zEjIfO?dzW5g7F(zn~YVH{4GDyioZR4fxej`JIaJ9afX~<-d?;WlzMON-MN* zAu?FSvPWE{QT#_B>R$8&{3m2aY6_(7U>MQSN^6@61yy6tA^Um?x0s6}(7*4jNxRjJ z4}&V)4!LxyCwU*9XSo$*!S*7X?oM?m||(pYKr+N z*&cl`nr!T`x*B>ogWsd5rq-GnCat_Z13+F@^E?E{ZF=HMp(Zc-Y5+=dFu$nctM|B%BsU(1?gkH$2d((CvqN$&CU~Du zcXQ5$HC0=P0kka#TXIq;HhG+p4+?>5ov-!4cT>Hly8RKP>GU$TU){4Q%wP((aB(Oa zIqfRL$%x}*N%Gzn>90uR2Yl&kVli^tDXg}N+s+S(Ds8vDVJlsm0M8v<_f)-=Rt9?8 zxEkrgOGX0KzRgFgC&5SX#4jmv#gFzfK8<&Y*8!6 zOO_6sIs7o`!Q7qHn`Hj$4T^Lzt|}?d9qV(nIWA!f8!`}r$nw2EySmF$XHs({3MPw( zD7NBt6t&PyoWil{LEXyp*|g{urSDH4Hh@vt!$eb8+ zbWjOb$Zwj4c0bZ}NTB;&fu<9|d; zNw7u`lYT;TViOpgIG{Amyks2Z|7P(`l2u_})KvMVf_Xy(%;woAipyk_n&$seN@p zTvDnn15K7u2|E3GS^APz*`n}QItRPELw3D!%s)N2LvNT>rLh$Wux2I(j;Atxl8qpyM0-vvD*Wn?*kSZ?-aij3Da}}`C!KR*&!(wYIfxOBO943cm7$B( z&*3#eJ zm3-TMT-NW_JlrF+HfHS@$j+}9n7C9UUU~Pp;gg}lag|UDIVCI2kdKsTD*#a6XYs%7 z%vZ!a1>C3VG94lQQ;-e$6MKKZVpgS46s#+BRtP#)&b|?eO`9TdB$u2)h%jY}s8=5U zqE~fk9ClnfZPJssI-2$gFigI}=o%)>_2DMC&aYEA=$y`{>$zs^dQYQFq!9F!du%ve zXzJ$`odX*t{l^Gq(G}DBghB!fbG>p>n>^q&pSsv3U-7)R$!66ea|(iDs;bi47=5A{ zftIgJ#W#~-3}S;R;}9!<)?yau35Tze%jbE3qYlM&80 zRoyl~A<|I#&Rlp4^QyopgXbQ6^dpZ~cAn1@)5-VfurqWi!+&{{3oA$lKa*@NOP39b z+AH)84L{$Qth4weOU`tIb^hC`dpMne3qko%*hiGqKaae-_Zo3gG4UQi-&|kkX)+eD zsB{T2J8%&sTtP1|eekq$?&oMm7StU3q;Q{)0$mx$aZaXL#_Y`;--mlXQ^ENOXn>Ni zTLLQEtd4Yak^?%L8Uh|1_Xa1Eu9RQ%R>}|MIf>lFZo7<1xyP|eqeW7H6zW0`5Ffcp z;h?EIfcD&cBQ*@CK=TKyLy0#>53TDUILUa%7kwbq#EVtWxEvTW<|L%wryRsK;m4$i zPsImaqd&<)+Upjcs(*dUkBjLq4uTNA4xn+0ZmTFG!bMx19wo-&0gWLGW?9>!4+Li+ zvIaYVU|=zWF`Zdd8-glNB271dd08fR7}D}%-0~l(zE^ft$n-EP&2%3ITGmZ=XVeeUqZbys!?{Bu0j?XH8x<3k4aetplmU~ zQx`r?8UFlC$`SAW0Eh}KUFsapO_QUv!hO8}qU)>|?ZFceWUb66@urh1#D^3QoqOW_ zfl}Z6-%9i9rYeP?fP*cSTe?frLx!`wcRy^yy7DqHLQzN=VS~fMC*Mjz*^=8mjL`$w zzd&1#m*)R8IrgvH>09_NonBA9X`y0Z$nR1Wx;q_r;Kv%fsT;n{+BltO=;Z2-qY3a| z_JI-Be-aRQOXD(foW0B0eCzV-Me}x1vAygQ$H_QMM|3ELS5+sL{G_AZ#`4cU2PCLy z@ZXpzab{a&?$HO~np_`i2%NUA9|N$)A4k$pkN^#4NNamlN3fq|9}hH{z*IXVU$*0sOO^Yp+~5Awt8ozyqZIS$l#hQ=)6Wh$AX zKV58kV(765i{AYxk{Gpexedbj!{tJWI>pdQX;^6%%;l;jy^S+a|CuOdo)9zOA6A3t zrED{hXw>8&V5?XRl^qX2tA8aRNGv^V#eU;Qmq@bDcVbLc`v67kBmxQspV0DfP%&Eb zQx&Y4-69wjYT`U=np41_m}Q|4AWoS<3Qo8eVy7X2YK&hOc@&^q2U0~8uBYdD7|nBj zCZvljw;D|O`sb}O=iZgX9Dq0fd_`FH*05}?Yw(*Svlt)}p#33|Td>%zpoE-*@A^5; zH%xo_v-ng>QoIyZ*FseGY(5avi_a2ze2s0u=nkY+?~0DE+i1aNmQS>NVZ~n>tmT4% z>ZK8gf&X)A3b`u;h?10mS}9IB4yg-(H-i96-o8?1)RrHr?%~h;r-Pyuj!gRqDP0QLs9 ztc-X#Mj25ONHXVxWT;6M&m z>of~h=T~(`=q}1bhemMFy>t+cH8f&h6@2K#nKg1{A_v;>#q3ESjSafjzPCr+vxwJc zo1^XL@gCcS`Or7IMZ#`(Y-jz^>^ao~d}vK<>Lu9xaf+|BnI8zUhVFs9jPFscp_Ce6 zTJ-)o1f3Xq1Rx|1`l5YRM!o4L2cls!-@Nx2!ZYpG`VxLxr7HWL?My{e8VN2y>HHGv zfvS!&?%KwQ->d_Z_w&BNc!?fws+Wo|az+#aWJJtIf zoWgL17;H4Kx`7NP?2Qk1NFjsLTHhw0KV3_-KTBAR@X<;)K@ZazLQfVY6D;vI$g?Nf z$QYQxjOQbpN4a$Li{7>t)bb)G%Jf-UP68hVL^5&1CpaK17|C?CQ_IECpyhDaVUejc z=PrtM!0BIjIC^*i`qmyPbX5drEr-NgF{rvR2}Db&B0HLbx9b_f@ge0798kx#m_Ht& z^Q3-TAYU#F(TW{ZM|?)EA@UoL(XCRTNqZx75k3gC*$5g@}S8!y>S#1hY&WcIXT*INxsG&IrCWc=2I z_Z&>fb6WzFfH5<4c6=(vC3=bU6fp&Q6u|BH&~&pNRUF<|uQhazMxzE-sAjrQ{pVk8=0+ zQM&72@Tm9L)=~ZMSAWJV<-Qg!|L2eD->R^dGyzw8+6v0ow%>`B-?Sc1*#UUg>$d-_ zcSwRdc7K^` zGgnvnfq0S_0^EO}0a)i!CxB`r1*w=sN@?#iz(*iGlO^#)h@WLg@ax_8k_;)pye?Fz zUO7G*oBr?V!r@^6OTP0wY_v$Lc&@2RQ0Q@h?&xTyzB%P0SXWE~*P|-T8|695s=$e8 z(sTsUl$+nqR{zswTTq}bj%QX0`-r3g#ss4Qa~S^WP%-Pt-3L_ zo(tfP<}+!?nS4&K1lj8!N8VLZY z9zWqG2^17!?#KTHKn^GyDWvj+*dA5+hh%8}7^$a{|E>ENEVLN-h!9$G^fTZ3-)>epi7?X#*o$FQY)Xy-qe-(FmVx5#GZqS&z`&a z?mV@$HJZBQ-uQ7as081ElNz>@Q=r%sRvUqq#N1LF$v| zd$Xx*T1DTV;*dUy-@&prOSPeZAH&2)D^c%`qPCkXGqke>G~H2emP1u>If0X4DSQsL zVTy-=zt3mvfW+*7y14Fms{8kiP7f)2W_PkPGE!Dq$2l^xvmPrH8QCP`WF9GGl@(cK zg|aC#J9}h=5{U;LD&hy8}_F8tdfEJDN#!- zr?=sp<25mKwq~#j{_?>_>taQZ*z1P-H{QocxJKNE>)H3orQT(e7OqYdgyFi$3ym;j}&)Iei_1RGG=ujkwE?| zImN*9jLEyXfV=Zy_qfvY79wN9(0Pw-)~0KBE_i*@eGM&+$Nd(uNLb2Nw*~_Nut&(0 zNDQ|nsLXK~6T;AnTLY?njs+PG5^c-HqdGQ?8D19ANh>bTKnvt^Ar0&BlN#^63<`CY4S}AajiR%IgZv+~tq2 z3xo{#8$=i38){2O#<-xg*r9(1Ptsv1SL000v>XbyWrGY)$t3*bf%w}Auhogc_v%oq zd}(*A@aY{$j(VSV=07Zt$011Hv|_mwon23J-O^|6<0Y+!o^|pm&V?NN`Reb{;?L5` zV%RR(%8~quing>-BSV)RR`;#`{HTHl*_p1zKb&FcxLFD%brq&z(Bq1*xQ`5gY`*eb z{ud*Z2Q8k!Ado8dZ{5T_z3Neq#QdiW(TU6@QlKVHyXf|aen-w27NbQ49Ub~2tnZX< z8q%UDwy2f#s8O>79T{c&7`QFmjG*D>sy&$n>VG099)iQ{PGNz^ax#pe0yOqp;2|_X z8FdK5SH1U-pFG8)_AwjnOY=QslNEpM%Hc@zeKOK?Hj#DBlt-Cn(VwDjxXKYV#Oqfj zRl^y)%%N9j&6_bQF+2pfR5q4GbAQUUzE8xmVPX}OSBuXdh)K$?mZ|bCar$GrJ?5Z; zDD_7%n-yCL?%cjX>YJlgC@|z zy`3r9b{$X7RkkXjw^f}RwCAOX;hCh^s1FajmGZ9-VN0z=b0dYn7(4|eXX<^cHIIvo zK+s{oY=AtQ4Rn*6^!KZbUY_sK_L2%ScPw&n?v{TM*c>^TY##2AM5`YOE(kjtA3NEh zgF8Y1*}Q5Bdca@jBvt5hQmwP=wq3va70oH|*&6L@M z+NbP6Vv}sw+%K50E_~+qj<4kgZ#w~58 zdxZXq{(Gpp^eCcEeBJiXgA#x?8fC+QkO`FN$|x2x(4#A*e-65eu5)jz!1-8y2eJ|G z=&bFrsRoSZf=7xE9=|idu!Wv+o$Uwd!;sOpLAH;i{}?2b!?ETp&y zrYkZm7I*M_aPv3k_g6TW@G-Q0APVn;Smvsm758FzvOqPG`ZVV`79V|#*@Cc#s;+)F zeRvfg$c==FC2x(8bk8bb8z%cDMyeNT0WE;PqA3`NgG9FUhL_I5Yr;87GzuF^q4sc7 z4W}x?-T~5zZ8VLZvU+zNMI^;94;M@+IJ1ua-hLHqs+nlqz4wV*o5?Z0o;y|i`#KB} z-@bfw;At+hfqRkxoGhMRs6kt^sp<9_PdY4>`a>I2G zw%;4&ZzKmH{WZzFBN@u%bwft{J@t@TlD1w^vo~dH;@^H4dK5pnB_sX~QkK@y??Z%r z5HT20Cg)7-Yh5WvaAhRzKzBYbO9J0zAw4jlxc#_f>l_?mr!VLo7JqSOZC*!x=Gg$! z?lk>cmtEJcl0JuD)gEs6SruOGiY|)Cj{n+|l7`TTaYHDo%&UJrVsdKC2XffQ+|W;e zI4$MbINY5z=R^3dV<+|vt|=y+GuZUvD#S^Jq(uGx*LTKVM{(N_QO7<7=yWDD=bKox zQVs1B1^6=c(vV_|?X6O}aIbrhg7s z_W(_+K*pG@0E7<$v#SbY83oJ)J+EE7Jtp##RNWeticM|CJg1E24Kx#qE9)ozhF4Yb zXbzBw-_c%yKgZoE?~z zjrdYA=eKoDRY}fmBBn++GyU`XWV$Bt+e+bxi%(d~)WyVU<*0hUAGU7SaoYDVe1DW`8E=u&!N1d;tde1KP9m zXH!t5`kS5ZYa=fc{Gp{^LY)3Tf06>PI6hLD=C_4729g9A=<)NB7YW*ECYSl~RGMLb z=7k{HNA)?F=9R3T@kv{rsjqffz}-Lm(ZTJO8MtHm>$LxX@JEbNsP@NpskOlky}{IZ zP-;{&@ggPXOsW6oVpWo8l3pAZBGN}CYj{0XZ#<#;sxqRc;)L$FU1LWDsq6h4w^V}2 ztSj~;mU*|T@uum~FZMQ<%#Aq{S30-s)-*$F1+y5cSvev7{omA~&Y`n^R4IsHR zV28}P>&KVIzX)Knw1}^^r(oJ$+X-r!MVQ37fI#&Q5Gc!nN6@MGprixk5Dq)yBaPf( zfI=>`gpcDT=Huzm9%LiXbd_W=JL1IDR?kDuW$}cRl?v8HQ)b-vtQzlnx9U@ePeO#s ziJveT@mN2~?QLd4%$cUT+-aI@W9j*nO!H$VRcva$gFDt8Ci)oC;0gKlh8q#5PTL}` zWq`kSTeunYj-cieYR=quFXcH8mSHQCrWJMs+4(SrUNE1^D&9WRPy-+Dr)L`(c4ZZm z#OIN@On=A#A^|kPT^dGRfvB>pgn=B~NlHrRG`qaIljGjkF>IwP@m|v44pz-9>~-r+ z_g8Ely$dQ|JI$d??j@mM>i$(}I$d(juu^u}(8L6OhVT0?mtFz$AqxoHMc=35zis&= zx{zw-6sn>afbFNb;t)JmK0bC7|NXQ925Rm?9LBC5rV7R^!12NXAXWqndx)Pnx^R1J zWX>>TJg#mRuH;h~AV|Az>EAA+y^tN5C~oy>zv)2$?w?Ed6ul*o=l4(6i$Fyvv8Mte zn-~ug8rWo^Mo^?+>v(XiM=!f81Az;8Y~T-_b6rjOXqOAyXnu2Wg0faVDk^k`3k zbUS`=Vib$l$yE5*a6s&lA>XcFfBV}0g;oN$*s>wq3AaamatFgUdpgO-yS5&(UzCBF zgY%s7K5x`yM%s#iwl~Thd>f>Jr4i?2rlkvdF-m=Djs&4P&5j(0IJbM=(DGBoJcEeC;CQ?Pk zMI3A7R{UUIR2`I{d&LRG>o{vqWy`Bv|}lFaq9&U*oL zvut~sPclvbeZ*4L!>dduI2lcuJ5qz9h+e3p{TD02FWiU{;5zS&k>7wyBee%L!l+0~fem`! z)cc|6YLamLTo*5iS}H3I_3zIHnex7+j(`eU1m(nN68YBXt5jcBBnmWY@deXC<2U$K z;Ly`AQW(Nx9gL=xup7P5gZb9TH0T+oBD3#6!b{&5GI0{*y2`FsW<$sLtqFgOs%;WN zNo6#n{Z%2cAsDHf+P+|!$~q=|mF392VZBun%NhBO0wR+5r+8WOa7w1)XN*>klFH?C z=fGz#FK{rno5_REk#rg+18SlZ0||Bp1kC0Y%XAjLzS(A(W-kR>TJ7r$Dot$mQ ze3GumA7~>PYxQ6eGb;hgC-LYVh`mBH>dSsS;(+vkboPAt{Jm0m}n-ya>C96eh? za>6EiR_-vi+zY%*MqXLXiaqF}=DCBh5+1@oHgy$LC7O|+Vzt&Z?J5x2e=fNKot=l8 z7BR6N16j(+-=6HkA(sbf`d+SKm-AiYxE}v`s_j{qj5Ozhjq)HFvvhZwH9O_M7&;si z+>)2*yeU34627(`rsNMIN$yl-7CMPi1T^L+7Y27EoQM`@oj>uD_pF1v=yeg}D!#+0 zcMWC23@2s{)O$2jmI%5-{lm&ifw{|Bc~krtlJ=ELiRXWtS{yft z0v{nS3ueEacF~aIF>cJ^CNS?*FS>rhuiQ|))Tz^GaXChQbZ^;bFBEFhgW54U2nuu? z6r`fWZ3+{`VjwCO5Fd}tC9@SRfWl2@;f35q6>}LwjY1@c&)D-U@yJ0AF4Ce%K%V#mWr#VyEpBJLgr)t$LWK2WLq=N>^!u9!7|!6*$`6LHw1SEf z(@SjAcN>I=2}UeSu8f=AEQF=G5C^TZ*4t%B@p6UY;K~x8_C-@^r+0{-rUphwtyIPO G{{H|kNPn*Y literal 0 HcmV?d00001 diff --git a/docs/modules/appendix/steering_motion_model_main.rst b/docs/modules/appendix/steering_motion_model_main.rst new file mode 100644 index 0000000000..9ae6fc5bcb --- /dev/null +++ b/docs/modules/appendix/steering_motion_model_main.rst @@ -0,0 +1,342 @@ + +KF Basics - Part 2 +------------------ + +Probabilistic Generative Laws +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +1st Law: +^^^^^^^^ + +The belief representing the state :math:`x_{t}`, is conditioned on all +past states, measurements and controls. This can be shown mathematically +by the conditional probability shown below: + +.. math:: p(x_{t} | x_{0:t-1},z_{1:t-1},u_{1:t}) + +1) :math:`z_{t}` represents the **measurement** + +2) :math:`u_{t}` the **motion command** + +3) :math:`x_{t}` the **state** (can be the position, velocity, etc) of + the robot or its environment at time t. + +‘If we know the state :math:`x_{t-1}` and :math:`u_{t}`, then knowing +the states :math:`x_{0:t-2}`, :math:`z_{1:t-1}` becomes immaterial +through the property of **conditional independence**’. The state +:math:`x_{t-1}` introduces a conditional independence between +:math:`x_{t}` and :math:`z_{1:t-1}`, :math:`u_{1:t-1}` + +Therefore the law now holds as: + +.. math:: p(x_{t} | x_{0:t-1},z_{1:t-1},u_{1:t})=p(x_{t} | x_{t-1},u_{t}) + +2nd Law: +^^^^^^^^ + +If :math:`x_{t}` is complete, then: + +.. math:: p(z_{t} | x-_{0:t},z_{1:t-1},u_{1:t})=p(z_{t} | x_{t}) + +:math:`x_{t}` is **complete** means that the past states, controls or +measurements carry no additional information to predict future. + +:math:`x_{0:t-1}`, :math:`z_{1:t-1}` and :math:`u_{1:t}` are +**conditionally independent** of :math:`z_{t}` given :math:`x_{t}` of +complete. + +The filter works in two parts: + +:math:`p(x_{t} | x_{t-1},u_{t})` -> **State Transition Probability** + +:math:`p(z_{t} | x_{t})` -> **Measurement Probability** + +Conditional dependence and independence example: +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +:math:`\bullet`\ **Independent but conditionally dependent** + +Let’s say you flip two fair coins + +A - Your first coin flip is heads + +B - Your second coin flip is heads + +C - Your first two flips were the same + +A and B here are independent. However, A and B are conditionally +dependent given C, since if you know C then your first coin flip will +inform the other one. + +:math:`\bullet`\ **Dependent but conditionally independent** + +A box contains two coins: a regular coin and one fake two-headed coin +((P(H)=1). I choose a coin at random and toss it twice. Define the +following events. + +A= First coin toss results in an H. + +B= Second coin toss results in an H. + +C= Coin 1 (regular) has been selected. + +If we know A has occurred (i.e., the first coin toss has resulted in +heads), we would guess that it is more likely that we have chosen Coin 2 +than Coin 1. This in turn increases the conditional probability that B +occurs. This suggests that A and B are not independent. On the other +hand, given C (Coin 1 is selected), A and B are independent. + +Bayes Rule: +~~~~~~~~~~~ + +Posterior = + +.. math:: \frac{Likelihood*Prior}{Marginal} + +Here, + +**Posterior** = Probability of an event occurring based on certain +evidence. + +**Likelihood** = How probable is the evidence given the event. + +**Prior** = Probability of the just the event occurring without having +any evidence. + +**Marginal** = Probability of the evidence given all the instances of +events possible. + +Example: + +1% of women have breast cancer (and therefore 99% do not). 80% of +mammograms detect breast cancer when it is there (and therefore 20% miss +it). 9.6% of mammograms detect breast cancer when its not there (and +therefore 90.4% correctly return a negative result). + +We can turn the process above into an equation, which is Bayes Theorem. +Here is the equation: + +:math:`\displaystyle{\Pr(\mathrm{A}|\mathrm{X}) = \frac{\Pr(\mathrm{X}|\mathrm{A})\Pr(\mathrm{A})}{\Pr(\mathrm{X|A})\Pr(\mathrm{A})+ \Pr(\mathrm{X | not \ A})\Pr(\mathrm{not \ A})}}` + +:math:`\bullet`\ Pr(A|X) = Chance of having cancer (A) given a positive +test (X). This is what we want to know: How likely is it to have cancer +with a positive result? In our case it was 7.8%. + +:math:`\bullet`\ Pr(X|A) = Chance of a positive test (X) given that you +had cancer (A). This is the chance of a true positive, 80% in our case. + +:math:`\bullet`\ Pr(A) = Chance of having cancer (1%). + +:math:`\bullet`\ Pr(not A) = Chance of not having cancer (99%). + +:math:`\bullet`\ Pr(X|not A) = Chance of a positive test (X) given that +you didn’t have cancer (~A). This is a false positive, 9.6% in our case. + +Bayes Filter Algorithm +~~~~~~~~~~~~~~~~~~~~~~ + +The basic filter algorithm is: + +for all :math:`x_{t}`: + +1. :math:`\overline{bel}(x_t) = \int p(x_t | u_t, x_{t-1}) bel(x_{t-1})dx` + +2. :math:`bel(x_t) = \eta p(z_t | x_t) \overline{bel}(x_t)` + +end. + +:math:`\rightarrow`\ The first step in filter is to calculate the prior +for the next step that uses the bayes theorem. This is the +**Prediction** step. The belief, :math:`\overline{bel}(x_t)`, is +**before** incorporating measurement(\ :math:`z_{t}`) at time t=t. This +is the step where the motion occurs and information is lost because the +means and covariances of the gaussians are added. The RHS of the +equation incorporates the law of total probability for prior +calculation. + +:math:`\rightarrow` This is the **Correction** or update step that +calculates the belief of the robot **after** taking into account the +measurement(\ :math:`z_{t}`) at time t=t. This is where we incorporate +the sensor information for the whereabouts of the robot. We gain +information here as the gaussians get multiplied here. (Multiplication +of gaussian values allows the resultant to lie in between these numbers +and the resultant covariance is smaller. + +Bayes filter localization example: +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. code-block:: ipython3 + + from IPython.display import Image + Image(filename="bayes_filter.png",width=400) + + + + +.. image:: Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png + :width: 400px + + + +Given - A robot with a sensor to detect doorways along a hallway. Also, +the robot knows how the hallway looks like but doesn’t know where it is +in the map. + +1. Initially(first scenario), it doesn’t know where it is with respect + to the map and hence the belief assigns equal probability to each + location in the map. + +2. The first sensor reading is incorporated and it shows the presence of + a door. Now the robot knows how the map looks like but cannot + localize yet as map has 3 doors present. Therefore it assigns equal + probability to each door present. + +3. The robot now moves forward. This is the prediction step and the + motion causes the robot to lose some of the information and hence the + variance of the gaussians increase (diagram 4.). The final belief is + **convolution** of posterior from previous step and the current state + after motion. Also, the means shift on the right due to the motion. + +4. Again, incorporating the measurement, the sensor senses a door and + this time too the possibility of door is equal for the three door. + This is where the filter’s magic kicks in. For the final belief + (diagram 5.), the posterior calculated after sensing is mixed or + **convolution** of previous posterior and measurement. It improves + the robot’s belief at location near to the second door. The variance + **decreases** and **peaks**. + +5. Finally after series of iterations of motion and correction, the + robot is able to localize itself with respect to the + environment.(diagram 6.) + +Do note that the robot knows the map but doesn’t know where exactly it +is on the map. + +Bayes and Kalman filter structure +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The basic structure and the concept remains the same as bayes filter for +Kalman. The only key difference is the mathematical representation of +Kalman filter. The Kalman filter is nothing but a bayesian filter that +uses Gaussians. + +For a bayes filter to be a Kalman filter, **each term of belief is now a +gaussian**, unlike histograms. The basic formulation for the **bayes +filter** algorithm is: + +.. math:: + + \begin{aligned} + \bar {\mathbf x} &= \mathbf x \ast f_{\mathbf x}(\bullet)\, \, &\text{Prediction} \\ + \mathbf x &= \mathcal L \cdot \bar{\mathbf x}\, \, &\text{Correction} + \end{aligned} + +:math:`\bar{\mathbf x}` is the *prior* + +:math:`\mathcal L` is the *likelihood* of a measurement given the prior +:math:`\bar{\mathbf x}` + +:math:`f_{\mathbf x}(\bullet)` is the *process model* or the gaussian +term that helps predict the next state like velocity to track position +or acceleration. + +:math:`\ast` denotes *convolution*. + +Kalman Gain +~~~~~~~~~~~ + +.. math:: x = (\mathcal L \bar x) + +Where x is posterior and :math:`\mathcal L` and :math:`\bar x` are +gaussians. + +Therefore the mean of the posterior is given by: + +.. math:: + + + \mu=\frac{\bar\sigma^2\, \mu_z + \sigma_z^2 \, \bar\mu} {\bar\sigma^2 + \sigma_z^2} + +.. math:: \mu = \left( \frac{\bar\sigma^2}{\bar\sigma^2 + \sigma_z^2}\right) \mu_z + \left(\frac{\sigma_z^2}{\bar\sigma^2 + \sigma_z^2}\right)\bar\mu + +In this form it is easy to see that we are scaling the measurement and +the prior by weights: + +.. math:: \mu = W_1 \mu_z + W_2 \bar\mu + +The weights sum to one because the denominator is a normalization term. +We introduce a new term, :math:`K=W_1`, giving us: + +.. math:: + + \begin{aligned} + \mu &= K \mu_z + (1-K) \bar\mu\\ + &= \bar\mu + K(\mu_z - \bar\mu) + \end{aligned} + +where + +.. math:: K = \frac {\bar\sigma^2}{\bar\sigma^2 + \sigma_z^2} + +The variance in terms of the Kalman gain: + +.. math:: + + \begin{aligned} + \sigma^2 &= \frac{\bar\sigma^2 \sigma_z^2 } {\bar\sigma^2 + \sigma_z^2} \\ + &= K\sigma_z^2 \\ + &= (1-K)\bar\sigma^2 + \end{aligned} + +:math:`K` is the *Kalman gain*. It’s the crux of the Kalman filter. It +is a scaling term that chooses a value partway between :math:`\mu_z` and +:math:`\bar\mu`. + +Kalman Filter - Univariate and Multivariate +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +\ **Prediction**\ + +:math:`\begin{array}{|l|l|l|} \hline \text{Univariate} & \text{Univariate} & \text{Multivariate}\\ & \text{(Kalman form)} & \\ \hline \bar \mu = \mu + \mu_{f_x} & \bar x = x + dx & \bar{\mathbf x} = \mathbf{Fx} + \mathbf{Bu}\\ \bar\sigma^2 = \sigma_x^2 + \sigma_{f_x}^2 & \bar P = P + Q & \bar{\mathbf P} = \mathbf{FPF}^\mathsf T + \mathbf Q \\ \hline \end{array}` + +:math:`\mathbf x,\, \mathbf P` are the state mean and covariance. They +correspond to :math:`x` and :math:`\sigma^2`. + +:math:`\mathbf F` is the *state transition function*. When multiplied by +:math:`\bf x` it computes the prior. + +:math:`\mathbf Q` is the process covariance. It corresponds to +:math:`\sigma^2_{f_x}`. + +:math:`\mathbf B` and :math:`\mathbf u` are model control inputs to the +system. + +\ **Correction**\ + +:math:`\begin{array}{|l|l|l|} \hline \text{Univariate} & \text{Univariate} & \text{Multivariate}\\ & \text{(Kalman form)} & \\ \hline & y = z - \bar x & \mathbf y = \mathbf z - \mathbf{H\bar x} \\ & K = \frac{\bar P}{\bar P+R}& \mathbf K = \mathbf{\bar{P}H}^\mathsf T (\mathbf{H\bar{P}H}^\mathsf T + \mathbf R)^{-1} \\ \mu=\frac{\bar\sigma^2\, \mu_z + \sigma_z^2 \, \bar\mu} {\bar\sigma^2 + \sigma_z^2} & x = \bar x + Ky & \mathbf x = \bar{\mathbf x} + \mathbf{Ky} \\ \sigma^2 = \frac{\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2} & P = (1-K)\bar P & \mathbf P = (\mathbf I - \mathbf{KH})\mathbf{\bar{P}} \\ \hline \end{array}` + +:math:`\mathbf H` is the measurement function. + +:math:`\mathbf z,\, \mathbf R` are the measurement mean and noise +covariance. They correspond to :math:`z` and :math:`\sigma_z^2` in the +univariate filter. :math:`\mathbf y` and :math:`\mathbf K` are the +residual and Kalman gain. + +The details will be different than the univariate filter because these +are vectors and matrices, but the concepts are exactly the same: + +- Use a Gaussian to represent our estimate of the state and error +- Use a Gaussian to represent the measurement and its error +- Use a Gaussian to represent the process model +- Use the process model to predict the next state (the prior) +- Form an estimate part way between the measurement and the prior + +References: +~~~~~~~~~~~ + +1. Roger Labbe’s + `repo `__ + on Kalman Filters. (Majority of text in the notes are from this) + +2. Probabilistic Robotics by Sebastian Thrun, Wolfram Burgard and Dieter + Fox, MIT Press. From ba7c7bdd09de8171c58177f860244c9518964b8b Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 1 Sep 2024 22:40:29 +0900 Subject: [PATCH 2/7] add turning radius calculation doc --- .../appendix/steering_motion_model_main.rst | 341 +----------------- 1 file changed, 10 insertions(+), 331 deletions(-) diff --git a/docs/modules/appendix/steering_motion_model_main.rst b/docs/modules/appendix/steering_motion_model_main.rst index 9ae6fc5bcb..9fd4d524ca 100644 --- a/docs/modules/appendix/steering_motion_model_main.rst +++ b/docs/modules/appendix/steering_motion_model_main.rst @@ -1,342 +1,21 @@ -KF Basics - Part 2 ------------------- +Steering Motion Model +----------------------- -Probabilistic Generative Laws +Turning radius calculation ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ -1st Law: -^^^^^^^^ +The Turning Radius represents the radius of the circle when the robot turns, as shown in the diagram below. -The belief representing the state :math:`x_{t}`, is conditioned on all -past states, measurements and controls. This can be shown mathematically -by the conditional probability shown below: +.. image:: steering_motion_model/turning_radius_calc1.png -.. math:: p(x_{t} | x_{0:t-1},z_{1:t-1},u_{1:t}) +In this section, we will derive the formula for the turning radius from 2 consecutive positions of the robot trajectory. +As shown in the upper diagram above, the robot moves from a point at time :math:`t` to a point at time :math:`t+1`. +Each point is represented by a 2D position :math:`(x_t, y_t)` and an orientation :math:`\theta_t`. -1) :math:`z_{t}` represents the **measurement** +The distance between the two points is :math:`d = \sqrt((x_{t+1} - x_t)^2 + (y_{t+1} - y_t)^2)`. -2) :math:`u_{t}` the **motion command** - -3) :math:`x_{t}` the **state** (can be the position, velocity, etc) of - the robot or its environment at time t. - -‘If we know the state :math:`x_{t-1}` and :math:`u_{t}`, then knowing -the states :math:`x_{0:t-2}`, :math:`z_{1:t-1}` becomes immaterial -through the property of **conditional independence**’. The state -:math:`x_{t-1}` introduces a conditional independence between -:math:`x_{t}` and :math:`z_{1:t-1}`, :math:`u_{1:t-1}` - -Therefore the law now holds as: - -.. math:: p(x_{t} | x_{0:t-1},z_{1:t-1},u_{1:t})=p(x_{t} | x_{t-1},u_{t}) - -2nd Law: -^^^^^^^^ - -If :math:`x_{t}` is complete, then: - -.. math:: p(z_{t} | x-_{0:t},z_{1:t-1},u_{1:t})=p(z_{t} | x_{t}) - -:math:`x_{t}` is **complete** means that the past states, controls or -measurements carry no additional information to predict future. - -:math:`x_{0:t-1}`, :math:`z_{1:t-1}` and :math:`u_{1:t}` are -**conditionally independent** of :math:`z_{t}` given :math:`x_{t}` of -complete. - -The filter works in two parts: - -:math:`p(x_{t} | x_{t-1},u_{t})` -> **State Transition Probability** - -:math:`p(z_{t} | x_{t})` -> **Measurement Probability** - -Conditional dependence and independence example: -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -:math:`\bullet`\ **Independent but conditionally dependent** - -Let’s say you flip two fair coins - -A - Your first coin flip is heads - -B - Your second coin flip is heads - -C - Your first two flips were the same - -A and B here are independent. However, A and B are conditionally -dependent given C, since if you know C then your first coin flip will -inform the other one. - -:math:`\bullet`\ **Dependent but conditionally independent** - -A box contains two coins: a regular coin and one fake two-headed coin -((P(H)=1). I choose a coin at random and toss it twice. Define the -following events. - -A= First coin toss results in an H. - -B= Second coin toss results in an H. - -C= Coin 1 (regular) has been selected. - -If we know A has occurred (i.e., the first coin toss has resulted in -heads), we would guess that it is more likely that we have chosen Coin 2 -than Coin 1. This in turn increases the conditional probability that B -occurs. This suggests that A and B are not independent. On the other -hand, given C (Coin 1 is selected), A and B are independent. - -Bayes Rule: -~~~~~~~~~~~ - -Posterior = - -.. math:: \frac{Likelihood*Prior}{Marginal} - -Here, - -**Posterior** = Probability of an event occurring based on certain -evidence. - -**Likelihood** = How probable is the evidence given the event. - -**Prior** = Probability of the just the event occurring without having -any evidence. - -**Marginal** = Probability of the evidence given all the instances of -events possible. - -Example: - -1% of women have breast cancer (and therefore 99% do not). 80% of -mammograms detect breast cancer when it is there (and therefore 20% miss -it). 9.6% of mammograms detect breast cancer when its not there (and -therefore 90.4% correctly return a negative result). - -We can turn the process above into an equation, which is Bayes Theorem. -Here is the equation: - -:math:`\displaystyle{\Pr(\mathrm{A}|\mathrm{X}) = \frac{\Pr(\mathrm{X}|\mathrm{A})\Pr(\mathrm{A})}{\Pr(\mathrm{X|A})\Pr(\mathrm{A})+ \Pr(\mathrm{X | not \ A})\Pr(\mathrm{not \ A})}}` - -:math:`\bullet`\ Pr(A|X) = Chance of having cancer (A) given a positive -test (X). This is what we want to know: How likely is it to have cancer -with a positive result? In our case it was 7.8%. - -:math:`\bullet`\ Pr(X|A) = Chance of a positive test (X) given that you -had cancer (A). This is the chance of a true positive, 80% in our case. - -:math:`\bullet`\ Pr(A) = Chance of having cancer (1%). - -:math:`\bullet`\ Pr(not A) = Chance of not having cancer (99%). - -:math:`\bullet`\ Pr(X|not A) = Chance of a positive test (X) given that -you didn’t have cancer (~A). This is a false positive, 9.6% in our case. - -Bayes Filter Algorithm -~~~~~~~~~~~~~~~~~~~~~~ - -The basic filter algorithm is: - -for all :math:`x_{t}`: - -1. :math:`\overline{bel}(x_t) = \int p(x_t | u_t, x_{t-1}) bel(x_{t-1})dx` - -2. :math:`bel(x_t) = \eta p(z_t | x_t) \overline{bel}(x_t)` - -end. - -:math:`\rightarrow`\ The first step in filter is to calculate the prior -for the next step that uses the bayes theorem. This is the -**Prediction** step. The belief, :math:`\overline{bel}(x_t)`, is -**before** incorporating measurement(\ :math:`z_{t}`) at time t=t. This -is the step where the motion occurs and information is lost because the -means and covariances of the gaussians are added. The RHS of the -equation incorporates the law of total probability for prior -calculation. - -:math:`\rightarrow` This is the **Correction** or update step that -calculates the belief of the robot **after** taking into account the -measurement(\ :math:`z_{t}`) at time t=t. This is where we incorporate -the sensor information for the whereabouts of the robot. We gain -information here as the gaussians get multiplied here. (Multiplication -of gaussian values allows the resultant to lie in between these numbers -and the resultant covariance is smaller. - -Bayes filter localization example: -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -.. code-block:: ipython3 - - from IPython.display import Image - Image(filename="bayes_filter.png",width=400) - - - - -.. image:: Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png - :width: 400px - - - -Given - A robot with a sensor to detect doorways along a hallway. Also, -the robot knows how the hallway looks like but doesn’t know where it is -in the map. - -1. Initially(first scenario), it doesn’t know where it is with respect - to the map and hence the belief assigns equal probability to each - location in the map. - -2. The first sensor reading is incorporated and it shows the presence of - a door. Now the robot knows how the map looks like but cannot - localize yet as map has 3 doors present. Therefore it assigns equal - probability to each door present. - -3. The robot now moves forward. This is the prediction step and the - motion causes the robot to lose some of the information and hence the - variance of the gaussians increase (diagram 4.). The final belief is - **convolution** of posterior from previous step and the current state - after motion. Also, the means shift on the right due to the motion. - -4. Again, incorporating the measurement, the sensor senses a door and - this time too the possibility of door is equal for the three door. - This is where the filter’s magic kicks in. For the final belief - (diagram 5.), the posterior calculated after sensing is mixed or - **convolution** of previous posterior and measurement. It improves - the robot’s belief at location near to the second door. The variance - **decreases** and **peaks**. - -5. Finally after series of iterations of motion and correction, the - robot is able to localize itself with respect to the - environment.(diagram 6.) - -Do note that the robot knows the map but doesn’t know where exactly it -is on the map. - -Bayes and Kalman filter structure -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -The basic structure and the concept remains the same as bayes filter for -Kalman. The only key difference is the mathematical representation of -Kalman filter. The Kalman filter is nothing but a bayesian filter that -uses Gaussians. - -For a bayes filter to be a Kalman filter, **each term of belief is now a -gaussian**, unlike histograms. The basic formulation for the **bayes -filter** algorithm is: - -.. math:: - - \begin{aligned} - \bar {\mathbf x} &= \mathbf x \ast f_{\mathbf x}(\bullet)\, \, &\text{Prediction} \\ - \mathbf x &= \mathcal L \cdot \bar{\mathbf x}\, \, &\text{Correction} - \end{aligned} - -:math:`\bar{\mathbf x}` is the *prior* - -:math:`\mathcal L` is the *likelihood* of a measurement given the prior -:math:`\bar{\mathbf x}` - -:math:`f_{\mathbf x}(\bullet)` is the *process model* or the gaussian -term that helps predict the next state like velocity to track position -or acceleration. - -:math:`\ast` denotes *convolution*. - -Kalman Gain -~~~~~~~~~~~ - -.. math:: x = (\mathcal L \bar x) - -Where x is posterior and :math:`\mathcal L` and :math:`\bar x` are -gaussians. - -Therefore the mean of the posterior is given by: - -.. math:: - - - \mu=\frac{\bar\sigma^2\, \mu_z + \sigma_z^2 \, \bar\mu} {\bar\sigma^2 + \sigma_z^2} - -.. math:: \mu = \left( \frac{\bar\sigma^2}{\bar\sigma^2 + \sigma_z^2}\right) \mu_z + \left(\frac{\sigma_z^2}{\bar\sigma^2 + \sigma_z^2}\right)\bar\mu - -In this form it is easy to see that we are scaling the measurement and -the prior by weights: - -.. math:: \mu = W_1 \mu_z + W_2 \bar\mu - -The weights sum to one because the denominator is a normalization term. -We introduce a new term, :math:`K=W_1`, giving us: - -.. math:: - - \begin{aligned} - \mu &= K \mu_z + (1-K) \bar\mu\\ - &= \bar\mu + K(\mu_z - \bar\mu) - \end{aligned} - -where - -.. math:: K = \frac {\bar\sigma^2}{\bar\sigma^2 + \sigma_z^2} - -The variance in terms of the Kalman gain: - -.. math:: - - \begin{aligned} - \sigma^2 &= \frac{\bar\sigma^2 \sigma_z^2 } {\bar\sigma^2 + \sigma_z^2} \\ - &= K\sigma_z^2 \\ - &= (1-K)\bar\sigma^2 - \end{aligned} - -:math:`K` is the *Kalman gain*. It’s the crux of the Kalman filter. It -is a scaling term that chooses a value partway between :math:`\mu_z` and -:math:`\bar\mu`. - -Kalman Filter - Univariate and Multivariate -~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -\ **Prediction**\ - -:math:`\begin{array}{|l|l|l|} \hline \text{Univariate} & \text{Univariate} & \text{Multivariate}\\ & \text{(Kalman form)} & \\ \hline \bar \mu = \mu + \mu_{f_x} & \bar x = x + dx & \bar{\mathbf x} = \mathbf{Fx} + \mathbf{Bu}\\ \bar\sigma^2 = \sigma_x^2 + \sigma_{f_x}^2 & \bar P = P + Q & \bar{\mathbf P} = \mathbf{FPF}^\mathsf T + \mathbf Q \\ \hline \end{array}` - -:math:`\mathbf x,\, \mathbf P` are the state mean and covariance. They -correspond to :math:`x` and :math:`\sigma^2`. - -:math:`\mathbf F` is the *state transition function*. When multiplied by -:math:`\bf x` it computes the prior. - -:math:`\mathbf Q` is the process covariance. It corresponds to -:math:`\sigma^2_{f_x}`. - -:math:`\mathbf B` and :math:`\mathbf u` are model control inputs to the -system. - -\ **Correction**\ - -:math:`\begin{array}{|l|l|l|} \hline \text{Univariate} & \text{Univariate} & \text{Multivariate}\\ & \text{(Kalman form)} & \\ \hline & y = z - \bar x & \mathbf y = \mathbf z - \mathbf{H\bar x} \\ & K = \frac{\bar P}{\bar P+R}& \mathbf K = \mathbf{\bar{P}H}^\mathsf T (\mathbf{H\bar{P}H}^\mathsf T + \mathbf R)^{-1} \\ \mu=\frac{\bar\sigma^2\, \mu_z + \sigma_z^2 \, \bar\mu} {\bar\sigma^2 + \sigma_z^2} & x = \bar x + Ky & \mathbf x = \bar{\mathbf x} + \mathbf{Ky} \\ \sigma^2 = \frac{\sigma_1^2\sigma_2^2}{\sigma_1^2+\sigma_2^2} & P = (1-K)\bar P & \mathbf P = (\mathbf I - \mathbf{KH})\mathbf{\bar{P}} \\ \hline \end{array}` - -:math:`\mathbf H` is the measurement function. - -:math:`\mathbf z,\, \mathbf R` are the measurement mean and noise -covariance. They correspond to :math:`z` and :math:`\sigma_z^2` in the -univariate filter. :math:`\mathbf y` and :math:`\mathbf K` are the -residual and Kalman gain. - -The details will be different than the univariate filter because these -are vectors and matrices, but the concepts are exactly the same: - -- Use a Gaussian to represent our estimate of the state and error -- Use a Gaussian to represent the measurement and its error -- Use a Gaussian to represent the process model -- Use the process model to predict the next state (the prior) -- Form an estimate part way between the measurement and the prior +The angle between the two vectors from the turning center to the two points is :math:`\theta = \theta_{t+1} - \theta_t`. References: ~~~~~~~~~~~ - -1. Roger Labbe’s - `repo `__ - on Kalman Filters. (Majority of text in the notes are from this) - -2. Probabilistic Robotics by Sebastian Thrun, Wolfram Burgard and Dieter - Fox, MIT Press. From 3c1dce86c59e67b5a8bebe8f5620107310ee8ae5 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 8 Sep 2024 21:03:03 +0900 Subject: [PATCH 3/7] add turning radius calculation doc --- .../appendix/steering_motion_model_main.rst | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/docs/modules/appendix/steering_motion_model_main.rst b/docs/modules/appendix/steering_motion_model_main.rst index 9fd4d524ca..0793ece490 100644 --- a/docs/modules/appendix/steering_motion_model_main.rst +++ b/docs/modules/appendix/steering_motion_model_main.rst @@ -13,9 +13,23 @@ In this section, we will derive the formula for the turning radius from 2 consec As shown in the upper diagram above, the robot moves from a point at time :math:`t` to a point at time :math:`t+1`. Each point is represented by a 2D position :math:`(x_t, y_t)` and an orientation :math:`\theta_t`. -The distance between the two points is :math:`d = \sqrt((x_{t+1} - x_t)^2 + (y_{t+1} - y_t)^2)`. +The distance between the two points is :math:`d = \sqrt{(x_{t+1} - x_t)^2 + (y_{t+1} - y_t)^2}`. The angle between the two vectors from the turning center to the two points is :math:`\theta = \theta_{t+1} - \theta_t`. +Here, by drawing a perpendicular line from the center of the turning radius +to a straight line of length :math:`d` connecting two points, +the following equation can be derived from the resulting right triangle. + +:math:`sin\frac{\theta}{2} = \frac{d}{2R}` + +So, the turning radius :math:`R` can be calculated as follows. + +:math:`R = \frac{d}{2sin\frac{\theta}{2}}` + +The curvature :math:`\kappa` is the reciprocal of the turning radius. +So, the curvature can be calculated as follows. + +:math:`\kappa = \frac{2sin\frac{\theta}{2}}{d}` References: ~~~~~~~~~~~ From f893f028f5e901adef3828b712ed5252f338085e Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 8 Sep 2024 22:37:43 +0900 Subject: [PATCH 4/7] add turning radius calculation doc --- .../steering_motion_model/steering_model.png | Bin 0 -> 72443 bytes .../appendix/steering_motion_model_main.rst | 25 +++++++++++++++--- 2 files changed, 21 insertions(+), 4 deletions(-) create mode 100644 docs/modules/appendix/steering_motion_model/steering_model.png diff --git a/docs/modules/appendix/steering_motion_model/steering_model.png b/docs/modules/appendix/steering_motion_model/steering_model.png new file mode 100644 index 0000000000000000000000000000000000000000..c66dded87afebc082d11743b5bab8b7531da1d20 GIT binary patch literal 72443 zcmeHQ2_RHkAFp|oB_c^FTcu6J42Gmcs6?fVEisIp8H};7FH2GJdTB?ik|C5Ogh;#= zEuymKNy@J5>vyi1%U~+6*X!kbyk40*_nvd_`Tx%Ezn*gs?b1+XW8r0)F=GbXwyj$< zXUst8&X_SXmuVI_!qYT&2>dgXsHv(rBd&1ChZ!?!lpU3I9Bp09ENx6@2&0rJzX>D7 zEeJ$MVbm63Bob?9Cx)}cnmb@^iDGzDM{o#yk2N>NQ%_JQxLMlRV1WV zVU)74_-4>5^arIx9T)Jy5=S}7X_ti)N|j_m#8}&@pwxAYNvhQD@K|ua!y~$dWNe8i z8roqUEetjHXpV0dZrs*I)d6c~p-#XJ51+XM+z0jEp!*+lv(OoPEZyLP!}p66Hz!%* z;OC@X=|~{hI9l3KAD(*9gn-AJnowm*J%)8~AUKab&5U3}eN>v;*fU|^pqtaS#5n>C zKp>JqqA3Fk9p7qdX>I{`Bq1$J`NkFu2jyhyjT0@fID#|HA=;dk47}K zU&ANax+t02KtP1Yk=hTy%U{2Cor>Ou+*IIf8E$oh>XKP50PgO`ubq0dT-E3rAZU@JRyviY3}ngllH$ zVhRMPh{F;spkIc+B|16~tViU90$&3{uu&q|5FqU133#XnO6OoYIhwkFqu?W*K^Q*t zWl4<6L)8?_VMhm7AQQMnG<6hRDLo)1Bq`lG(_o2uiUkdfWT?lmRIr+lvB(%h#Ykj5i;K zinthB93?G*LQ05BN=pFFlmTa$;&CJ24_`0|5Kxc;BSRR8Qc)2HKj89dctA*_0}mAm z8%JdchrNnU+huVW7oBa2rZ$s-2P7Vde}Q~WY_LQiSun~pQ`&(*!sCXgFf{xVr|?)& z;Y`i2BpbkzRA5kBCIEJUkEwt;4?2VBf+9>)+&B$mhaIK6Z=LBhGg*qxOoop+nA%_+ zEuA2Ak1`Ft>HX#DN+Fw3%GpH#rpXuhMw-eua8E=6$-%^wde!8(%)XHK9zx|l{$Dd0b(ucTnOju$2k%3jE8ikgSl|&(>#lU07Ij}s5azZP+z7lmX11r{l&$kAlZXYy3i-;?^_^)apVIu`$m=E z80!QDe!w-8RcJ_Y3gd$Y!6lzJ%fHN+qxxoqjFI9rlmN2^EhWImevIOiM2*!#DDi(> z@kz;!?Q+DnpYn>2I&_nq3g2YR!LwnQ2dQ%b{{3@v0kZ$UkC``iR?yWS!^(fE&kCt= z?k5V35|gF*p@wo`zU})(((9U~>3-@SFLtLjyMi%wHnZ7UeLd!_j7uAZ`JjPNRdyx=2(V zMJM!ur8?jyAlL(i11EUqww5>?(bfizS*7Dg1&gTOEaTGNOaj!4j0A&}h!W(KgSDf0t%Tx6(3 zGvyFL4Ik4W0v7%-7yJ!g!NCoUN70vQ3tUAdF!0+jL4i=oG#mkgLw}$E*+e)>z}7Dn zj==Auz;nvMQGzpyBQ zgEZNg;g*Cy)2JgUs73)232hJuDNTL6f5l>^Kj4(R95er++oo}hXySB#IXD5uT`4_$ zFA@P1Cu?BxgYA8(;bP) zWdHs~MjMn~{a1MghKdYzh+!WIR!T4i{sG=WQgs|mb<`>@xW_3=JCiU%zP+>x3Lx&G z)@Tt4Hc+{lBF@qQ)L&6(2*?R4b)&-hD+N__=i1+99XKZqLrI((SEHdF>JZVA&QE9C zP~rF`?LpVJq0~lAzPJX2eH~nNK;(>)h6W#WX+Ml6V4(B1K!XY?n!jfjA;l@PixRJ+ z3xla`K8?^C)kB3jX1s0l{b>zEKgXC;BhjZR&ubII&lJw)us)6 z2^lIlV5>}mN;t4u`u>^_RL+gngs_z1E~l)8FY$%A_unNsVI^f@MWAX&gVz5=ZbObs zA;az|4Yf@TbqL4O$1od3w#2^+70iDgB|njsG+grjnYChY@%-qLJKCV$@VD@4zp-Dy zM@QF{*~zFYZE>}i-RofGtnK1LFtjI9iTyKc0C#9YBuv{ZO|E?OhQM0B={10#S>%UO z`ixvDBRM*rjlG2uvt`cY$6Ga>vse@Ef(VWye zq5t?ySq{eDl!GHRN;d|MP+bCTk<9;@nT3S4 zAXRuHp&%OJ!SemN&8*2}0sr5G2hTKk=1BpxLV%re!lRdq7&r5N=(=Mmq_hlLToNUL zmXel6gLN|F)*Yjv)u>}5*Pq!hWvprbqqoW!pW{SMS%grL{UvVxp$PfPPAXz3Bw7|N zD}fS6q0uO5SrmF|07e21)WMVjyDU?VQs^bY|4m8(TkfD_q|q|6QYaZ2v;<(5@uVUH z*1G`2pvBQ>UCLlOw^8Ld! z6S{=`Yq-m)5TII@$4rMQ>r!)PN(s z+{jM`M{tYNaHP^C{bk$Z;fR7uLVf~v{6WcnLvv+yqAFU{%t6#vXPfg*rM+%crX5Z8 z@3FJ71Y4+1=lCHn>_5HFFNP@YWJ^+VA{e7wwN1=qTxAShM3Iw-fzdc1)NN|UY~cG@ zSA-H3{NmEg;WCDa#V7P_b{Y$bW{$%p`?MAieC)^UXC@^f3-*Z{4uQg!8XUZaZTau- zK1e}h1Yw%mCH%;6k5j>Wpo>Vl6>-pm(&R%-NDfXT8%R%t=VuWKPNi-%OWCoHW|y)t z8%=*5v*31ycW0%nkfHw|!k1$RH)sd#%uA%NBqwqa?Gbvh1bFgOXXeQ5f4$I)wq^KUC&VgHu#pXu4Q%*)r}oEh z&j@$-*P0(TfhglVxhVjer`et7%hyl~Ccka}U>V09u1?bU&8C~xWRaVlBneJPQ_+c~ zB@}k0A)UXsD3fMmP*W*TqBFc29IT6?Ic_%mGZL|<`9=}U!r?0@H}#ceo8PKbsf*%J zQe%~wgfulnO`~+-8A+>j;bZWm`k`S5xw zIR*vYGNgoI;eLPCAKxV$9SvG%*TIkyW)JZ$KH91f{;|!Yr@b-|f z_JXw~6=*bM11lg}vVrjlqvMB?4P7wce6Fk(2)l(;>k!f#IkTj2Ji= zKjoMX!WN{iz8IGM<^_7RuWJH}tbX#uo+P{h9962Ykuvp!Ix))mpPi~w@JkJ-LWTEn zn|eZ>jHNlciT89R8l6+=5xkCU-ar}6kjJWeQ`lcC1P|2cjf-=*ZZ9>3Z5`nStkkTR37tf%q#MLbHP zsA!-XZ%7$xVCJ90g8! zJ!c1jU`Kt+f|aSGqbv2Lxmc1T0n}&G)R)tHH}>dT<40f40A8Ct@@|HaH#$&HgkIdB zOMP#{@OPt^jd`cT#6tJ6_3V`SIGI=*#SHvH?K+a`VNuZV)f2y5QU}L(Xv%w$;*(yQ z0bTvI2-5d2+Jo!MCTabRzPjwMzTRW9Li>gxrf~sbD~L8M4Ii7dBqnnUqp}`v3c^80 z*iWSfX8t**8*hBVh9TVJlw~^9#8lnH@W-g;oyO*<4hHa~=QhL8_+i)_ZxzBfF!iws zf~{YTw@zadkRQE2bg=m~qd~IZ+;Ca-yfUfSH1rK z@Fu+lXf%6L!0#@i9-8Ea!PXSySU}8$ z3jdK!Y^dm$f`ijE7S+i6O~x4Uu<7-$Sn+hl_pJdpnk3fPEZA5nL9a%o`tha?mS7xB z9roB^O#uA>;f(B}L?sC6;mu2Ec2yz(-vT~H-FXRmjrnI~JT7_l4HBl2AgoJh1qdJe zK}ious&rtS!SAVr;q(0?B;)NiDVVY0E~i5HjxNx@HF`G0&NUpGIMQ| zQ^#fZP;ej)20Bcra8KVK=#sDr^SLgkEYRtN0>}BKG^GHe>AlYa{l7xDD7d_k3JGaA zZt~Be5GhHeB^ZTp{WaX>lttlRE61Kj;fQgGl93gIw@mwSK>R8LPgx++iy4li!yoz% z9vKwa@Kr)m8YwFwC4-if0ro6J$9VH$-&9;oRz?afAt@z^l#!K010ekES2j5i9I=j; z1U%K?{w$mSXa@&nUMXr#nv)bp5kNBuSGn`%be_?P8KNl~rh zsYhYdi25WcjJizxv!OA1D`@B#GXH0JiGLwbhJt4@94M2d2AHNkMbS%4&=i=^@h<^| z*^>yv9wh8Z!mh$39gokFngdLaP-W8?QX4h%{!)?MRHHi7xY(PNG*v!RkLvzfB4C>8 z=&kKRp{Sz>^NtkNqJVkl`&(lu@E&E_-IJuiYq*C$!(C2YYwWKj1*Q>67m-jH6bfLE zAd(spfZ_2EAQCk$OErEH{6vWSL3MVP>RW7fTk5IpCFo#Owe7apVJ!40Vi~Qo+v8{o zYFt6=Irk@=e)yix5*A;Rg9>Hj+rQ0V~>WDq3C2nFJtSloGmOJ zO`%#VsCL5{RB?is3XLB_0qcaE4^n)B`|`P=9zUY06YJ#0G8G(x9b-vRPoYf$ja2c% zjP^s5KqivgZLPPeY}ul;dpk~Zr;^^*%{wMj7 zh$DHjoZ3J+F;q#R;K7CKJ1JM4cb_Q~)3;{^wUy~F!B9>Q!t@}Fn=4_g{T+rw9j%pJ znOzaOew)r_@_V*Ft8T7HPbqY4TzM-zQ%p<@KiJXwM8q8B0j5^@GC`cI5R>F8akeem z+S<9@E!B|;)$coQcn`!P^*f7<4(&KJ!(#m@NkIwTcN-~ zg+qp8VQz7|%KLc613bPah`=XWx6agV5K=Ij53Xiq@LFZJFfK~G^R~m8b6g9%s^%3u z+1As-&L{*I{5sk@u4 zej+(_8(-xrfmy3;xV2aOvNUvyLc#W&Yg_>J_gI&(>g8ZEB2O?_fjhB^K5j2IuXRJ3 zvQ6bJ-|TMNdnSgdt+pj>EN?g%q&YYer4^hGdNM?Xq_rjWdRo1?R>#X;j=pUfq_`9NSJy1#SSN_f)3?-3&5nM&B^Ny_7RSARL+&yK zAB6+pK@KQ4*>BIs+_1@iT65Z{5a`BLstN^bt}akX++~#Zr1+KpjeG0vwI{D($XUmI z^5jXa!^MKk?P~>b9s33X-fg_|hx|T|#;A&2_Ug6#zV^i>>*i_9LDT|B)xIoXTE<{g z%;9@^TL&UgAh>U9M(?pR3N@h98b)b(1Vib(coPr(4#~6002s*))=mt700b?hboTDC z(gc`IB-^S#P|vh2F0y&fe7I|=hWo82nDvJ8oH`w;3AU!_+WY zkUbE8#V0iBO0r2fx1Cap?ou|$L^wYWDEniI9pXFCZJoPJiOa!|Z31_{DM`#LswlK} zR^^`$O?|jo{6hYeXSXm2D zOI(oP$_2ix=VIC{;$b7RahLofqkfmlytGWW0L~cw*CejQ2XP_KIDuJnJG&a~s`Qj) z=;pDobL-s$6){eKYR%?I#I(77TH@P!K;Gl#hmRi}3d?Q@D+nIQPLTiLN1pTabo%^6 zlbqutBYCykf<(<#^)(n@69ILd z#}3aqRw)^k%RS6XJM8v|&p+)kcUkhC(4<&rXH~BFG?ClP+H2kz-%+o=PjHPYjoDF= z99{PO5|gf4ZAJ^($A@%c>o15rrc|9e(Y)@*g?7j72v*%)9Jfj`U)aVPRd%uJx_YKJ z4kX_=!3X$*OOcvfs1T`t|im6&YbT)!%?O3&ev zyLK)5^>#)L19HB6(uPlVUf8RKYh^O#lKNlSHKEu0lY2<7Kk_M-oG9hvv+l1xR-bv` z==&XE*vU6J-il5$`o@?=1A;(;cNqD`23}IjmDnc&$7;w(sXKgJm1ZV zYLie^BmByf$kM~=XuMDn^SVP~Jvh^5sa?IXs6|DgcbTVlg1uct#k<-SP6h>a1nBFav>0U{YtVK-7tq{8}>i zK`3&cI%|6Sif$sQ?Co3o(BBb?Cw@2iExWXjX#wAhvrHTXD>#S=5wl|XTb}S{*`7Ah zKXXNY{k<2WW!o;*+yo2=rU5%6yZ6(mSGz)xGZB3(_x6)>tA<+ZJ9;juX`eH`o_X5>EGut);Lop0zQ65V_mMbDh2NL$y5LUCH0z@#a(cfYF-pWGvTv z#q2z{{ik2rq}w%bCH7)ch-dl+orSCohpdh^pH+s`B=g#Nb_o&r$!2B#MAb7zDQV$F zn>!i-(nSV!dSb;Y9-+f z!+MUbqy+X&BEUWgh|g1MxN)US>|W}<&hYDa36tk$nLeRXRpc^fmp8tdNX1fO132nr_M=zE_>mJyY=EUaLNyoguJf9F{Txbc&U9z;j3 zzI%BZa@Rd*Z%@F+T{SFsI^cOdP0IV%7Rk**)+=2CA&_iCtX@|l-yH3X&$CpIB{ayH zR+Jom<{knFxo{4E8OK6RL$Z6uCXqVz)~mf5254{Y=RkQ_%EvU@{*k*aCL;bWmtgBf zh5F!*>H%GO51WPQO*zMknqQ!}_Pn(H<5fo8EnspqrgEMdRA_%{zLL>*kilUeIe&2u zu_9)#ahStkqFdvvz9l4)XGtXz%s?xNwWVHF>r=!S@2lXvrjVVt-r%=F+w}PMtu-fE zqGEonPu@#B+r_L~Cd1=-JeTl`fuSLf{Qd*W6a?=8_^dB_p}+nxIx~=yKaSxp{BDtVNRHW%1oYyTdAuftX=d|c~xWZmJKJ;&_W_f7N!IKTt*%}r{WX; zTqNe}2V~DI;q4M>-7PHbaVzC5cVC7MN&jMcg}WcQi5x6&-lsMujKXB?Q>`&*HDAack%{wfd2*)J~eckLY(q3RcR zwnY=zt8y&x6y1=v1eUhs zQE;z53dIPbSc#fqPPLWBb@Rpivyv@2b0WM~V}dp3JIy&$!?N$XL2KO$(Yy`q5<7wA zQ+GuQXsR>w58lxUskUgo$uze`eS42hZtnSxHwWgjW4T2bPJfV=a>w5~*HCsMfn@#h za;%Z2B8E{Jxdq_!?E)F+1dW8HjxJpNrN!hAL3*u=v=7~vABw6L@wnVwmNig@z8iDp zqBR@hPui!s4X@i;E<89TCA4X_4C& zy2qIbi@jENR>ZIuJ9@I{epnmayt;peRTDb(;$60*3Z{rao`&PqR@)Q6Brdz}*xmZp zq{}KP6>x0W0FQdXn)Hp^RL}iNlf zzz$7K_>dqS=sxJ*_P`c>;w4We z^P=K76Ji;W_+01GDJMnD;pbjJyA3p=WfQf?>Zkbp6gnWSR>#kL)9m@xUjAGyrp=wl z)~{QO3rCmU^bbwC>h;b?c-Fwy1Z@4w>W$jbm^k+Q*A5((uK4IDDe}Es ziJiNu-kicD02`MHG+yqMP?&H=>Q1ygd4DaFJtmQN{brHypinK#YW!lS?rQFcEdrPu zIS)_r@~%x>&AD~0oqoak3eeUtz8g*Ea_C? zVu;g^Dm=<_18hJ<&y)9vt1b;x-6_I_O3@Q}4H}RAqx!bFaE$-I@-Ig5HEwH)}^bT@j!k~#8h`xKZe5+$=SM?oTwU(GOsFbk7XS43q-nKn@ zy4pt(Gi$HGJfK>nePwwXB>MeATBGK=YqV|1wye64^#NRh)`~!-oM^z5mFq3M5n8Os z{n!`8zC;+`G5q#{rRa?N6%965w*v$1I%;Uq@Cl8-h)q9NiQ_%W$FE+k3~oslAptbl zow%Ged*VX5F|qeEbM5a(+7h)N;?o0dj_!=MHL1tczj9xAGPGDBOkVM-@6s~}-*85J zx=v*2R!4|fi{wb>TI-0p#V#9!L{#>l-3O>r>@`~-oAMGvHNu<4< zV_zL3^BFeUcUsled9?^@nTk$q z(@kcA(!4}zIsYZ^4}y?Go27(79!pW{Zfstjq$5I(^>9|azk6iPN%ME*zevrBIu*HD zb`$Rgl~uSV4`Y&-i%`s;T-mDr+&oX7GS6QcY9$vyMBgRw#i{HUiV5Y4Ey6PCa`RNO zwG5xoxYo`_F5pzywXy&{uN8b{&#`}~hDN*ik@p_!)mKrcNRK%VRQUMV+!V`oD3*{c3`!E*&@4#-}UWp)q5g)U$oY1XSt=Iu0`2pEPj4;_JTyGd(IX> zr(B+|^GP(b6xd({?W)e-s-wfm>%7{6bFB^gN~^1e2C4#Q3wYRde>|DBVMBjwF1nV< zw|=G9pDP1mVq(rNZpi9Py20k%#^WvF(SAvQFT(ZR>9g|8Lk(Gj)-DbDRqHc8hTT{) zSbt-1jp-uu4lVr(Gxa#%`-+%flqDGv48040LO6D0i|TD~0|^3)B3N{*`YO8uPTpxW z%@r7G6PQcz?kfx2>GkpV+{l5~k!*%N53~cIyAr7Ee}|rnO=^g~8o@i*R5jT2E>d8q zJ3r8Su+H174c8Z2)f-zaFjy@Ru+H;?T6(xoQn4-iS~&Y!ToXe}JQ!ph5phq+$EAd} zVyA?o0|l+g?unJ{QRoVf7d#uK=fr68@bKgkpU9&#+OMqR+xYRfkU1gpov&K6f0;Wc zvisgjykd!a<)?`10|NbT9&g9!RJegVNB2WrwjOoPFtJ_Dla-a6CK_RJg)l#Hjp#Wn zm*CSI3XSMlbxg%B2}gTCkD(pabG==<8_hSl-<(IP^6X-JvazqR^$5Yk#N}mrrBwgJ ztp0~~LjzrNBL~~B5-@3s1-(bAx{sXYj%#J?l~1p-@zPrF-l7iywH`%tXva$mSHg9OPYBQoSsCRnvhK!J^IS3_y_y4hTOZOBYUH(*A0DK7l_VmJCX}@wD+7%pHQ2K?KL@BLw#8S zUOgwhUAvzQ^*^b;>fL+QS=P{+Tqd?lj&E;4DYMrf*sEQF<+q7-c>-(IdOz|5_nfm} zQJfXKt^{|6DDF;c4r^}6PN*jjmFmts$0s`qq3;(Hp@ccrr(gN; zL^Uz8ue`OrXy@FW8`|!%sX}J0L+jbBp0j9ldWeunIgw%!cfjUzWjtmteb>v(M8q(@!!J+k^~n{#<(9B9WzmPAY%RXwMRIw6ArQYR|7`ux+ns|rm$I@-r%&eIt+Tybm-XTNMFn&EhUbVf^Ma0N zRuv1d^Sf6F#bjSxviP|quIF0(v5SjV_?||nq34CWp1Sl{|Bg;1tM2N?&c;i{X-$@A zKE49s#m9#|^E`rA)KrtyeH6@mr4}EGTBsMpV=W|-_xeJ}a8T|xTNS!u?usSSjnDoB zJii!m#lu>s;QA*It0X)LQ=_UXiqqP)O@JG-Yq`>L@jZgIDi=|6J#V>!Rz!4H7qWY< z1>wXK4{WbJMn_3|2 z^0-2Dwbc@jCZ2{5>n;`PuOZ`;7dHZnk#smy?GM* z<;(Xd6qN3%N&qof%=r^`!nk5jmXb__ZpvWKGgjTYBew)W)3>tu7avlY+k7G*$&h@t zDklQ}%0_tffn{4Dr+$mklOtR9%vUVfej(LUAnB#8dxCAqDx7UYlLZtsi|T1`7NbMDOqpp;ux-t3&8@Dlw@OV-b!lrTSiGb}2y!cSm4P`vKhZx_>Q0Ymd8%%~ z8R7zqvw9$~DS9S8>%#zJg!U;8j5dQA%dI7GDnSbrF{jk7Y3bzg8Y-4N`rY8ua0IpR zRyZ?oJUdj3;uPjqUr!1p>6i#vH|yT;+&)uCr3!ES6n0i`FSy=! zW3z}zC`r4z{k=a~QAF;JZ;{dkR?%Zvsg=7Lz!0!JtIsQ?eg4x`D{-c?O5}YHDKr5z zt%?c_Q?&2fs25w-dCc|bDqO9-K}AD!V0c`!&TJ?P@iYgtvb}UdD|Jy_kFwKR_Mg83 ziD3^#46!A)?tg4houBwIHLjlIR^rc<{$~gT(q>)Pt(!=V>?mfz3IqhjmwReJ#%aYZJtL~SU=78 z|4_&!{}}Z6&eal3+nEUIdFqA?LLzrQERaveq=-IB-ViVxTICds-wqm$>Bf5!t-(aP z^amMND@SPeNC!sKt3}94!;kMTg;XAH{P;03o|Bi|w~5iiL_EtiSV-g=2oT+b$ZH

B=42M# z$CYSYn^dL;&$q5J~b@#E>(b^g)= z+vjfeM*t1wXoX)@Xv|wLE@a6xV+M~7R11)7K+ZWcm~-0+1T`Fr4GV%|e|5SIhHYPB zukh2;K6YBpcTJ#M*Ey0jAm9E}39?t5+k5;JF*e+{9tsQMYL6AK^VI+kS*mh@a%Nc4 zz!Af13q#PJ?_xr^^%_2whD2iUz`sFxMOwiS+BJ+1FxQL zr@a~ApsksAkWCH%;c8}bR(#5vmVwF^wkMm0`X38b{LU?MqhbLF`z?5YI?B26*F}c6 z(NE4wtW^g(VxORN*ODhfSDtQSp)>^Ke>$0<-uR1L`4i zl?%P??IryB5*;!%pDatxdgUZr7uQk*Tsy3JV5pYP<~djOeY5(~vlc_fzWci&$VAQh zD3XQr=*|`3-`^IbEosW-o^aHbC;0cYtXDNr_>*q@2?Ke_YVw8yc|iX58fTM$k9NsP z5Rt-iSf~NA{aX`H-mzf{G3|k@yJ|LqOK$66SC)5oMb$t>6obj|T(;Nj;&Cs3(L}wi z=(?CJ88BLfmPMHAnEV=%u6bQiTtsmMH#2AyGRy-`X7$&uJK9gW;a<_&nmc#yPWg_# z$~R>+P^vY#XufEnw0+H)4`^7$p-a3_3Jbg3WA8wgJ$>^GTaaCfYRP8 zC>A8@$p%zLlPzKK#=h52*h4lAy6jn+z|#Q6UM9LPSRP;R6kW$zW@rN%D>gjm6Utq8 z8H5PJnU?^Yk|U$H_y&K3XXoqc%)Vyp?aMoj`O3r=tR#6SpZa?&lcHyDmp zpGWzAkk`cuH}q8v4YvBP^T4{)$yLb$k1KMKSBJKD((2!apF-!{7au{R;`a{LtX9@s zlMs37b8h?h$L;|4{8X$%#3TBAr#9KJaf1njQ}=yToOY=AC@e!l3>Mg0R^J=aO z2=QvabmUR{>>|5_{n~|ha;~Pu#^^UYv+CaV?*J*{Foa1#FXSbi63+_JW~G;h64Aj(TFJxMG7gX~Ma3ca2_$wH+9tJsV0lt3(k5fKYQw=+}y zLze9fyOoqCDnknP3J%qBbe(e)7<6S!w;U7L-Pr=d$iSh{b#AGwm}MV$>DJy9zV1W; z&4LFS8-`|7yXw~jT*jU*3%LA=?GDJ2yCOsl$hl9R%7Ma!8GhcqsdkmO&y&^z#DtqB zM!5CP{B4f9DgzT47><~>rp|XK>XR26-~WYGcai-`r-~xYF3n}Y-V=;fP~&Nc@Y-z~ zjkkRA9_a?sd)IR1FL5NYtPK+4wAgq4N-!x^z&fLLX?aNTM^PTp;@G?_V?~UwR(Jqe zJmW;xqCJ9HLY(h4p%d3tt(~*dN{wXQwk{%6%RZRlp*dwX7MR_OJ! zuEcTc6-S&%XBG9ua=zaVUD+-9roO&D%1hMyCg8XpZ)IR~SuY3YEzDi|^qPX2P|UM= zeBme3!VGMMVxIQ4w>OQx#%g$RkkmBJ z+)@H0ceM}%@^)QSVC88L4{Wt25DWOSh6;*Y4J{P zJHJp8Z~>ZTS04n;tlQO>r&nM#I6v0mS4B*oETb|C1jVJf-?w>@)qVMFegiCe(6$WiA4YD| z4Nxpe?mP-+P7eYfvnosF&e2^3*Ux|4h$Nri8nD$T8;`hC8IrU(^%qr;{CT#wPilT* z=S^oEg5d$rserXH1GO(>u5z;IhV3o$)D4^eK%AN5m3lS?o&HR&l^IHW@5zOa12-jm zE~E%G{M&$k{W$00L$xY#@@0I$HXkrm20%b%-dBX0UB_}bB*d>S0*V{*~J&Q`{I1Sw)JF|w!M4Ls?2 zlUFp}63~~03+F0YF~L_ z)C!!Gj$Mo{+k)cbOpeQqxMMCEqc>^+WmFH9ZiTtz zFC_|Ydq-D(T1G~r(fz1b*TG|3nFeCx&lKC)s~>@$K8S}SF=*Yhk25v2Z`#zwmp_vm zMt3$S39&w;r`*E)u$f_yVd376B2Q);z-mnyH@`FMu)VK$i9ZG@we1#KpmruHG3AkU8drt@o-blX z)D9za-&`#+D1Hl9ecQ_L5kSOsUvP6r>+EvibOy#R%isasCv(IHCHW_s92W$WYL`FX z{WrG*Vv*#6 zmk~8yI?}Ql53b*20T0I#oZJyQppur9TElrwJ9|ZbM@eBp5#Y}c>Syci`+Y5k$Wafh zkPL}TF8j0>V8St=5K3!bM!C|a{v++Zz3aP#AmbztQT;12GiN}j`Xp){LLOW$uRZjr ze6F`hhf>iWiayz^XJ#FjMKkqZ1G#i^s#f;LEH1wpzC=Xe%!h0(b@%$a_5;Z5UvtMe z!pkQv`x6Fngtc3l+=e(?xdz~m+i(-1^T-7+E88fBcPy_Gv2mw5?|`wtvv>X74TeL# zFRM0eb@RB*xpC%oF(%2SMlmN1;E)B zI!0;!!F8vk_gU;w6BEPTTp+) zEkhGjqnbi?eN3g<+5I4pL@2cl2|OECss^e6@7`O?!4c29>f-7~&|K60Uz2AES|n-h zy!l}dxZ-BBSvchi&yZg+T~}xLJYdZ6$9mkJ9rMwccQ6jjrMUhk{(VoaQj*O|GP6^U z2^%FDTB9C;Dze&+rnIiy^R+Qm29fM53{NXC>4Bj50aXhvj7+ZRcEEqvmo%}N&C$dE zO1P11wgnoS6sVRWv=M=|o}I;JZtgT=m@9XHbIF1Mz_NspcuPnSm$X~;CWJBYnFUN) z7LkM1V88`a6jHU39)2xt+PANCRDsw>ao-}NK?WwrWgN{RbE&Z2<_(BStj zK$@5QrDOjDBK&jv^OrvTP8HgB9y_`Fn#_E7y{(gEcW)zr!p4UE<>a}}fn72oe{RPq zaI?5PCHgC)-bR7Tq8{jW;rj0zI1e1o+~E6RdqT*c4`V_&lRzP1l3~qtv-jwh>kmxh zxtaXV1Gn8YJ%Gi^^k5SZW>fL4-bLyMPPnUHUw0J(%XxjkZe`C;%D34I2t3)aHeCa< z;btP>D8leG6+8f-GgIp%_)s7!-dx$-CIg0MrxBRTN#%udtQ<@{yaKBi>|#=Swf9Xc z>(XX}^5a!^nYIj@HM}QQi0`-Xkh*j0!NQ$lzPaXekLI^I3!Y5%VkU!8FX?gZI*O=% z8=kq(!f9=Qk~xn|a!7gi?hD1`js5HjM#C1>$vp` zLtO<-=uU-IF;&ez6wM!^iQ7%IN`A-A2qNQ$)lVVmBzWcOINYpJrG zt3V;94vCn_$+XftHMPg$8FRvw>}WRSrqWDP?j^z5(x4yd!lRm}va3@#W0%jzoSzj) zT(i$Vdi@aZ+SJvY-PP+pVapn=tL`~$Y+r8V9UF1|&d|OP+ZDVjrcYu9tW6jg99VSz zcn}@a@+YHT?4N2c?#-C{1XX?gP~sT|Uuu<@8;8t-WO8%ouAZiM3%y>kXqUOl&GVfH zkOU2B9@Am$LwRK0@yn-uyQQs%t&e9w2TFzo+<_2eZ*PCI{*Ce*X6h- zELaWb<3+KvGoq+iR7AJ-q<2P2eK~rYNm*gOfDkXJYhQbn!IzJby`{sx#-rExaG9uI#M~_Ig@cdK zzZ9Kq_-%%-03i56280~*x!!<39c6?Zb!+wZpC{h0$@jw~o>=U@5}s6`8I20bO?=19 zSH+1ut+uLm`P7>lloaO-B{IOEE)m?7k?rD*S@^y?0sH9jr=qT;w0fyKC)acz9^&Ux z$QLZTuHLpXdFH(LI@#MX`FZG$L`LJRXHHgGaq-3|uO%3hY?+j~y4ND|Umy2mU(DxQ z3D~6a$&61o_6x2!{79FR)wi*ggbzND+Zl*H6&Sc`r=i4!mSlsH{7gS`{Z2!WBLpR6 z#g4A(akG8fYU0~WZkbe_4`$Yl&e6MWC)s2^$F(}rgz4Pkem5DON8rg50ge*uv63L_ zw4k}d%LzMf@yWgqV1EA%yXyJ9z7ogz^qChAtzEH19o@U6 z&TZjw`_ From e2f2f9c121d03cce595861c8852155a3f6d42cd0 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sun, 8 Sep 2024 22:40:42 +0900 Subject: [PATCH 5/7] add turning radius calculation doc --- docs/modules/appendix/steering_motion_model_main.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/modules/appendix/steering_motion_model_main.rst b/docs/modules/appendix/steering_motion_model_main.rst index 336dc33cf6..4fb79f7959 100644 --- a/docs/modules/appendix/steering_motion_model_main.rst +++ b/docs/modules/appendix/steering_motion_model_main.rst @@ -16,7 +16,7 @@ which is the distance between the rear wheel center and the front wheel center, and the assumption that the turning radius circle passes through the center of the rear wheels in the diagram above. -:math:`R = WB / tan(θ)` +:math:`R = \frac{WB}{tan(θ)}` Turning radius calculation by 2 consecutive positions of the robot trajectory ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ From ea221d96b39153965fb6f181421d1c5a1cd8f59c Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Mon, 16 Sep 2024 21:55:40 +0900 Subject: [PATCH 6/7] add turning radius calculation doc --- .../appendix/steering_motion_model_main.rst | 49 ++++++++++++++++++- 1 file changed, 47 insertions(+), 2 deletions(-) diff --git a/docs/modules/appendix/steering_motion_model_main.rst b/docs/modules/appendix/steering_motion_model_main.rst index 4fb79f7959..04b2c198f2 100644 --- a/docs/modules/appendix/steering_motion_model_main.rst +++ b/docs/modules/appendix/steering_motion_model_main.rst @@ -9,14 +9,38 @@ The turning Radius represents the radius of the circle when the robot turns, as .. image:: steering_motion_model/steering_model.png -When the steering angle is tilted by :math:`θ`, +When the steering angle is tilted by :math:`\delta`, the turning radius :math:`R` can be calculated using the following equation, based on the geometric relationship between the wheelbase (WB), which is the distance between the rear wheel center and the front wheel center, and the assumption that the turning radius circle passes through the center of the rear wheels in the diagram above. -:math:`R = \frac{WB}{tan(θ)}` +:math:`R = \frac{WB}{tan\delta}` + +The curvature :math:`\kappa` is the reciprocal of the turning radius: + +:math:`\kappa = \frac{tan\delta}{WB}` + +In the diagram above, the angular difference :math:`\Delta \theta` in the vehicle’s heading between two points on the turning radius :math:`R` +is the same as the angle of the vector connecting the two points from the center of the turn. + +From the formula for the length of an arc and the radius, + +:math:`\Delta \theta = \frac{s}{R}` + +Here, :math:`s` is the distance between two points on the turning radius. + +So, yaw rate :math:`\omega` can be calculated as follows. + +:math:`\omega = \frac{v}{R}` + +and + +:math:`\omega = v\kappa` + +here, :math:`v` is the velocity of the vehicle. + Turning radius calculation by 2 consecutive positions of the robot trajectory ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -46,6 +70,27 @@ So, the curvature can be calculated as follows. :math:`\kappa = \frac{2sin\frac{\theta}{2}}{d}` + curvatur4d)neT[BtQVp2Ue change by maximum steering speed +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +If the maximum steering speed is given as :math:`\dot{\delta}_{max}`, +the maximum curvature change rate :math:`\dot{\kappa}_{max}` can be calculated as follows: + +:math:`\dot{\kappa}_{max} = \frac{tan\dot{\delta}_{max}}{WB}` + +From the curvature calculation by 2 consecutive positions of the robot trajectory, + +the maximum curvature change rate :math:`\dot{\kappa}_{max}` can be calculated as follows: + +:math:`\dot{\kappa}_{max} = \frac{\kappa_{t+1}-\kappa_{t}}{\Delta t}` + +If we can assume that the vehicle will not exceed the maximum curvature change rate, + +the velocity :math:`v_{min}` can be calculated as follows: + +:math:`v_{min} = \frac{d_{t+1}+d_{t}}{\Delta t} = \frac{d_{t+1}+d_{t}}{(\kappa_{t+1}-\kappa_{t})}\frac{tan\dot{\delta}_{max}}{WB}` + + References: ~~~~~~~~~~~ From 0e06664e7188e0a96cbf5d22efc62368148127b8 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Mon, 16 Sep 2024 22:00:45 +0900 Subject: [PATCH 7/7] add turning radius calculation doc --- docs/modules/appendix/steering_motion_model_main.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/modules/appendix/steering_motion_model_main.rst b/docs/modules/appendix/steering_motion_model_main.rst index 04b2c198f2..6e444b7909 100644 --- a/docs/modules/appendix/steering_motion_model_main.rst +++ b/docs/modules/appendix/steering_motion_model_main.rst @@ -70,7 +70,7 @@ So, the curvature can be calculated as follows. :math:`\kappa = \frac{2sin\frac{\theta}{2}}{d}` - curvatur4d)neT[BtQVp2Ue change by maximum steering speed +Target speed by maximum steering speed ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ If the maximum steering speed is given as :math:`\dot{\delta}_{max}`, @@ -86,7 +86,7 @@ the maximum curvature change rate :math:`\dot{\kappa}_{max}` can be calculated a If we can assume that the vehicle will not exceed the maximum curvature change rate, -the velocity :math:`v_{min}` can be calculated as follows: +the target minimum velocity :math:`v_{min}` can be calculated as follows: :math:`v_{min} = \frac{d_{t+1}+d_{t}}{\Delta t} = \frac{d_{t+1}+d_{t}}{(\kappa_{t+1}-\kappa_{t})}\frac{tan\dot{\delta}_{max}}{WB}`