From 9b27c44921f7b6e23421d644f3e3368b3f8d99be Mon Sep 17 00:00:00 2001 From: Markus Mayer Date: Sat, 13 Jul 2024 23:05:04 +0200 Subject: [PATCH] Add test data from serial-sensors(-proto) for gyro-free fusion (#2) * Add test data from sensor-fusion repository * Add estimation of data rates * Add simple visualization * Add simulation time * Add basic simulation step counter * Add display of accelerometer and magnetometer * Add debugging of state NaN values * Use stationary position dataset for testing * Incorporate sensor noise into filter * Calm the simulation a bit * Change simulation frame to have Z point up * Add sensor readings to display * Remove TRIAD vector re-calculation * Incorporate axis changes from magnetometer * Approximately correct filtering * Add updated test data * Move estimator into own modules * Add half-ready gyro-free estimation * Add somewhat working example * Normalize magnetic field reference vector * Add more refined test set * Make orientation order explicit * Add Gyro drift estimator * Set stronger trust in estimations * Fix missing factor in accel measurement update * Use filter-internal rotation for visualization * Fix issue with accelerometer display and rotation * Rename variables in quaternion normalization * Add correct Jacobian for rotation * Add arrow rendering for example data * Move text rendering to own function * Add pause by keypress on space * Fix resetting of simulation times * Use submodule for test data * Apply clippy fixes * Make test estimator deprecated * Derive UniformArray on all types * Resolve unfindable type --------- Signed-off-by: Markus Mayer --- .gitmodules | 5 + Cargo.toml | 12 + .../data/fonts/firamono/FiraMono-Regular.ttf | Bin 0 -> 170204 bytes examples/data/fonts/firamono/OFL.txt | 93 ++ examples/simulation.rs | 856 ++++++++++++++++ math/quaternion_jacobian.m | 42 + src/accelerometer_noise.rs | 36 +- src/accelerometer_reading.rs | 54 +- src/euler_angles.rs | 39 +- src/gyro_drift.rs | 6 + src/gyro_drift/filter.rs | 290 ++++++ src/gyro_drift/types.rs | 79 ++ src/gyro_free.rs | 4 + src/gyro_free/filter.rs | 752 ++++++++++++++ src/gyro_free/types.rs | 97 ++ src/gyroscope_bias.rs | 120 +++ src/gyroscope_noise.rs | 101 ++ src/gyroscope_reading.rs | 62 +- src/lib.rs | 776 +------------- src/macros.rs | 129 --- src/magnetometer_noise.rs | 36 +- src/magnetometer_reading.rs | 62 +- src/num_traits.rs | 81 +- src/test_estimator.rs | 966 ++++++++++++++++++ src/vector3.rs | 53 +- tests/data | 1 + 26 files changed, 3642 insertions(+), 1110 deletions(-) create mode 100644 .gitmodules create mode 100644 examples/data/fonts/firamono/FiraMono-Regular.ttf create mode 100644 examples/data/fonts/firamono/OFL.txt create mode 100644 examples/simulation.rs create mode 100644 math/quaternion_jacobian.m create mode 100644 src/gyro_drift.rs create mode 100644 src/gyro_drift/filter.rs create mode 100644 src/gyro_drift/types.rs create mode 100644 src/gyro_free.rs create mode 100644 src/gyro_free/filter.rs create mode 100644 src/gyro_free/types.rs create mode 100644 src/gyroscope_bias.rs create mode 100644 src/gyroscope_noise.rs create mode 100644 src/test_estimator.rs create mode 160000 tests/data diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..5e3db18 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,5 @@ +[submodule "tests/data"] + path = tests/data + url = https://github.com/sunsided/serial-sensors-test-data.git + shallow = true + branch = main diff --git a/Cargo.toml b/Cargo.toml index 7436eaf..0159f0c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -11,17 +11,29 @@ categories = ["aerospace", "algorithms", "science::robotics", "no-std", "embedde edition = "2021" [features] +default = ["coordinate-frame"] std = ["minikalman/std", "num-traits/std"] unsafe = [] +coordinate-frame = ["dep:coordinate-frame"] [dependencies] +coordinate-frame = { version = "0.4.0", optional = true, features = ["num-traits", "nalgebra"] } minikalman = { version = "0.6.0", default-features = false } num-traits = { version = "0.2.19", default-features = false } +uniform-array-derive = "0.1.0" [dev-dependencies] ensure-uniform-type = "0.1.1" paste = "1.0.15" minikalman = { version = "0.6.0", features = ["std"] } +csv = "1.3.0" +serde = { version = "1.0.204", features = ["derive"] } +kiss3d = "0.35.0" + +[[example]] +name = "simulation" +path = "examples/simulation.rs" +required-features = ["std", "coordinate-frame"] [package.metadata.docs.rs] all-features = true diff --git a/examples/data/fonts/firamono/FiraMono-Regular.ttf b/examples/data/fonts/firamono/FiraMono-Regular.ttf new file mode 100644 index 0000000000000000000000000000000000000000..67bbd4287d424a70c582c86278bfa742a6bead6c GIT binary patch literal 170204 zcmdpfcVJXS*Z0gVo1Q@E)r9~eBw>@yCILc8r4u>{ASNW65J)41CPF|2L`NUdEb9N!tb6b=bSmy=iWPWmvP2eZCngi zt4C6D%D~?J|6oi*AgkyxAa!tuc>{|Xv(;s+-jW`JhbBZn)jx)@<|iT4AF+{-*?ykD z7+(kejI4sp;y()?%wudxPsTc~$eLJY^+^qz1br2VUy@UtTX41RyWcYAwU{x#$8s}E zp_|o$d{a>G-25pyZ@pac6k}&TVEpVKW1X4VUS|%xiTJUIj~s5VtYkr;%sk6KU?bTm=7qv(#iAH; z#m@RY#&qI>#4|U4Mn2R2T=aXIv57)F|Cwh+h#qw?HzoS$Jwb6lGF|_SINq$A`32Dn zIUlwBbJ0zIW0TPsUP?v&)3r3dn3>Q$UrR$!A4&OW7Rlof>Sy{;Bs01*ZLY>*t5QxJ zl9a%XvVh7;X`B8G_sL-F2~w*CX*)rz!g|J1D2(|-i;sDqIfHSt!mMDLIn$g4_x81xC%J>EL(x9}RoG(mw@qOORn`IU6WR`^8Nw%gsYe&w#G2S8GwY_w2>5I37 zSsI(d=aXNhne#!cf5Ji5UHl2Z&XRVv;`0Wkxw_9w+o@+K?QBE7{oWNQr0(;w zh9Hn7rQ_e;j^3@k>v-w@?Ox!O54enncWRI|v#IR|vh7rNh7a89w1vveS+F)uxiuE3 z6)LyRqP1A%HdsrozH*zaG5xsPZl;)E{Y-VWaC9j1&}Tj zw#2g@|43I4Hiiv?zJIDS1*w%F6)Wopn+gG|u-_`e%0wKMCRtJftEe<`a2=vbmZYXa zB43oxU-Ku+G92YD({ZN~1}mMY$8sT8!iI_bR+qgUB^b$UEV@e4)52S2t9d)4jE1lz zmcTk!$^Bi5x9V`)SDi`{EY1bWfT^&K+EzA7p-hwv)owAHf-)g4Y7thp86cY@a1B!~ z2U`5!mlm}~*}tz>6O*%&2aQZ?5z?Z`{4}d;z<{(CuI}kgtgcvc#iplQcS@$r zY*$+ZDWBD4qj(!}9vzTog%R^JtuEgIX&DHyQk*ZjqRADV(If+7R}&X+pPnA#Vgu5g z>FHrEJ=B_Pbs4QQVUU?PAkAflB)BXg2{0c+Y(|*N2n~T<*6f|;m;@`u$i(UD(le5? zTzb0}2v4-mx6X&|oi?*IvNbR*V?dM4LFs8B>4@zbYjU!5=MH5?(*_u ziOC7=T2vLbPw1_M?<=+N_Ant4InJ;q&kxC@iV!8mnow!FtW98~+e(+-IwUhmCgJxt z;;vSZf^b#VJcazJ&4eWE^7GY`(^@nMX_4NpMVKofbf=~zyRtKr!d!u&$bi-A@=xqd z`GO-PA>9=~zCrK>z!&BUf|^>QNUSJ8)G= z&XCDruG*n}2B!5HECZXgKzJPy9vr%p1tktm+Zhy;=;E0PuAuhRNYQi?cKXvL02ddp zkDAb155QE5>Oo;8%t!T7vh7-gK+NrKBI8K}ZJ%P(k&_;q_hv=D6)y3>nj)6kvX zM4N@~^dWjzs5RWh?+$ad6OIXCuJ*!F66R_i%3J~M{|7eS0UMh`w-q)LZ2=pJwuFsD zLtrD(R{CxX&8p65+XcH=C9F_ zp;m{m*A70O{MR0hfuct9D4f;D?E(HJbq$H#8OiG*M^VT>46acm0}VSfHq7M+4Q~(^ z=8FDztZ0N;V2?qSu==g7;np71c~O+T=FjgD(gTA<8isWYi|EnOoY$+1^kXr~)JF%w zf5GDNNo?<&A0A@0#?6PWj@8lDaH+#(Kqt7Yt__HwY-5;L;VF%mPgF$`&mnN8r%NJrnjT_zJIVPFnP$&76h zf)rAaFA$Lm^wp3E457i&zyN@d|HJ174mVZeY)e`OsNBW7PxTpcTOP3l~ z2zo_0I*GI~34;d4@T!*T&hNi(ialy-82^*jN99K2OI%27lNKKJ-6CDtm`sJ?F4Ppt ze+w#kGEwEDD$6~P<$AJnVzx&=tP}2vLSO9pH{rc7=JL9ATn_N`4t2$V^r2EqM!8y3 zFs-@Esc$GXM^_&dYroK47-!(<4+kelYUnO5LI%JgLIx5~GNJ|%4><-C4>^Vq4>^Vs zPdpsMh=&}*iH96%#6ymB;z@*K1o4n#B=L}A6!DPbZsJLSV>I!Q;~wH6M+WhbBa?U% z;21+Z)cPF2O6LmpRG1m_A@H@L=%_JrW>@Z^QMI#+RzBah%7FI>c( zFI>c05bBDnVl5<(U@a0ZVl5UfVx17`>Qcp8LLR|dDqO@`CS1f?9{QY*p}7}N3GH3p zPM6+lz-0H76Gqd)AoVw!EoQ56@4FjMSI+Wq-ifF11$+g6Uu&U-X&to}w3FKFTBV+> z57g83Ir6i6uMn9v>IBJ|VE*YN~-pANC_AeFgjpdz)Jxa0zL?2fwsW7z@C9a0y6>^1wI#eB=Ah&+kqbk z1qU?`Y8Mn8ln~TEXhcv>P)X3apvytmf^G!etmRv)ey#bnme*QeJEL}f?YDyK2e%FG z6ucsMOYp%uP3lapGr!KSb(8B3tb2Feyt?IeXV!hV?&EczuGhBS(^ziXm5 zsng_+CJ{}hHM!ihdDC`HpKE%g>6vCto3(A0-K@CTv}W^~Ep4{0*|W`FXkNd0%jV(D zpSi>5jv;sKvPN3FT6-F3ZPOuOuM#qFlG zJJP;s`yTBFx4)$7p@pG~LLUo#BJ`Qiw?aP(vxUWm6^E@3J0A99 zxHWue_@eMvJ2dVvx{GE@V=s3M?l`#Pj*g#o zYSU>>=SH1dbPnr0we#N2KgG3+8y~kT?r_{Iac_08b_wkg+a;;XqAshtJk#a5E`N6Q z=~}OAi>_f^J9bU(TG(}J*H^kV=$6`TPPY%cM|Lmkekh*D`@|>4uZ#aMp>@K9gcAv8 z63!>qN-R!%HK~45PEujgi^5UNdF-Hs}V6H5=W$t7&#($MCphb zBQ}lLJyILlXJp05c_Y6a)os*_QE%K`=kCnA*Nl!EJ#F;VD zXGUf28dGP?q%qHp`8lgqR%+JltbJL>vR=-5BU{h*%dV3hot>3EK6`2QJ5KKOakh2Z zoUzWa&L^DDI4|e;<rTbMgFcWv&O-1B2w zjm;RleC&a-U*tvP&CXk#w>ob_-m`hv^L`o^Gw#uG*T=K*!Q)$vj~?H1{D|@Ss65 z1w9K!6yz67FPLAjykLF73kBy2J}LOEuvTH4!nndgh1rD@3+ENCEPS@`aN+BP9~E9N z{H@4R6kOD*D7vU;(TJk_qUlBRinVps9O;+Kjq6n`;+O{hO1bV9;}v5IGB(-E>$=jv&(gmfjmo+OJSoUDq z;j+_ZpO>eWKU98pV*QC@CT^Vg(WKgw3MM@{>GI^3lfx%>nw&Cu(B#pR$4#C%dDi5G zlUGjOIQiV<-=>65DVcJ1YP+eqQ+G`L{NDQa_PclWG|RNRr)|2=_r6Z|t-tT1`@XpE z#(lp{kC;AZ`jY8ur*E0QXZrE!=cZqter@`V={INi&Zs}5<&5wdU1#*4F?vSfjOjBL z&R9ER`;5ag&ds|4btYS<>Va3#nITcGP)>dq(*i&)5 z;#|emit80OX9mt}J~Mn~*O~oij-FXKbNbBrGnda?KXd!c{WDL`d~@d2nP1HOX_huC za8~14Pu}15{*Ugze*bT?Ewh7XH=o^ZcJ%Co+5Kman4L4bWcKvg^Jg!gy?*xg+52ao zp8e+RtFynD{nG;zANXm`mN|Ro9G`P;&gD7R=G>TbbFS~)`g2>(4WHX-ZtuAx=8l^? zb?*GRE9Y*RyMOL0b1%>RV(t%fZ_evKZ_2!b4>o+T*MrL+yfMG_{IdC{9%}wjuZJFb z=(7b)7ZfabdBN8Y4|w?Lhre0aec|ARnF~u7KCtkyg_{>1SorF~_ZNP*NMBTM(VdGr zF6y)B?nMQQ4la6kas9g>Jir?Z!YP%WXY2COO8F-@X^hWzPq&J(q2pRmQGpv z;L^vJZeF@?>B~#sS^D|XUzhnTYqadHWgVCGT9&>nZ`qV(4=#Ir+2&>Ymc6{}$K~G3 z+bvIB{^;@}kMYMM9xHw9#mBC#NLsOW#n~0#KA!ye@W;y^U--D|@efzVtxQ@uWM%fs z!>gLC>ayydRkK!YT(xJ_=~Wk2eYEP^RX11ruWr1$^XgHn^H)z>J%9C6s}HWexcd4U zuQm17gsh2J(``+UHDlMzTeD`(t~EE-My#E&cGKGTp7462-V-5DL_E>$i5^d+-uAa_ zon@VK-LWTIJvsBqQ|tZL$E|;O{j2LQu77X+r|Z97|Ks{UpE92cc&gr0&7OMvsrR1x zbc1a}?1sBH6l}O}!H}BhgcJrTG{I-N`N#2sbW!jbnTUKw`u;tk;ySE(Na&pVr zEf=<2+49MjueSWK<&Uk#R{yPaw>I6{YHR4$sI75ZleYHVTDbMmt-H5g+WPCWcRU;a ztn=Ar&%W~PciS3mOV~Dk+uUtWZ#%K={cYcF=i7s~his49p1gh7_T250w?DZ3@$H+p z@7w;$_II~`wf)Z>fjgS-=&+;vj)6OzJErYeyko9;e|rD(`(NGv>Hc32_#e3AK;(gb2eJ-KIk4!!(+Bn(IDO#51AiW@cQEYWz=L@Q z=Nx?M;DLkZ4_-gyd+5$XNr&vs=eR%NUg2N9SUUPWQ;d6(-J5v8h zn^k6bwN)luVU)1#e_jyyW;=<1^fkDfhx<>=Q(ZypOg*8Eu5vChW^ z9LqX(@3Do)HXb{A?5$&;A7{tw9B+R-`S{4=CC6tSf9&{{7vuKPOm(@?evSM-#h)|i{39snMn6OBt6QzqI?( z^>^yN)B2sR?~Hh7(>q_j^T*|Y%O#f&Uw-ZK2k%C|oB8g9cUQmr#ucwCaaYQ&y!u{; z_a1of-K*JG%dgJ4`sLL>-Vb;`<^78Hw|{v4;}IXfdu=*a<{a?l-&)2oEh(k%5Pm#A zv#gK>@}>=P`V}=YuNO!z%tiAgV(TfkPZ4T{5u)nhI2_Mju)2&LkEWiH+b&0r6)#~9vri&VT>UAzSdWwChA z(VvaL+kp}`oy}*<*?Ps{#p8Vg{JSZlmf>4G;O_uviGBz8Yr#8zP+0{#gedL!Ks@7irF4tHLo z**Lt(o5dEgm26{XW`3Db@}FO`NYA+cjccjX$j-|APp{eedAY{u|AlK|QGt<`{U2X* zN;0#I{{Q(EJ2Uc)p5mGyu5scTEv~e0Bj0EzuC2tixwtkI*WkkPf)d03Kfe}1g<<($ zxRzuW>Nku2<7;X8m{R?h|BY*LseYrh{6D>xp?vhO{`2cpXGxKM?LWU_7~mKq=vq%) z1H{!IR}CW}?a<*Ecj+1^uDZBt;)*RX2=@|K8c{g2h^w!-(yj@PT^)4w!&S$aR~us| z`LW9c*M{QS09OqoSrb7iyt#K%%O94{@OK)2pIPPsNr2V7Y%VY}%qY`pdgJNNEF;6{ ztCwh3wB6cDttDU2%P<2c9*Wq;u+AwXn7Nd36x@g_BWUaT>AHIM3*BNr2 ze_f#E-~V+(N;Ul@BYn@m9F@w{uR{;a-D->&b_UHVR)Uch2cj2PW< z8;??$hd+(Ii9eleVFsndOf6L#$}HM=?G+ZFKc^qW+#>7Gn@S%Nb(t?*278HV`g!4s zS1t{6$w^Vq24>JW%*cVLSq*d9An=V5WzR7C$#&9T=#q8P4DlS{w3n+DK)Y6`uH3vW z;R{wF*z1Ja8IFcQHnxBkP1r2`ULt)Bb?qy(H58I=ZAb-q#9l2(41k8g+F(eJ*XE)O zp3$~r{wE9mD$@-zR5DU#h>|gcObgAW?Zu2<-L`yD9?SK|5Tfa`^aUQ}*BD{c`uv%N z2ZNsEwQNbY+^w@GYoULtU(;{s-|IinJdi4&6VaVbG5nFe8Svm9yO17ST&AC>I?LT^@aK( zeX;%so~u5pFV&an|1vB_Q{yh9ozdP1HNuQ=qk~~HB8*65v~dsmVWu(0$TG4Gr;%gi z8e@$-W1KPG$Ttd%LZiqiHYOM)MyWZ_oM=umC!14{VkFA#etow70G{5?)#vFC>htx7 z@a(qIXkauq?l7!I3!|mc%4lu0G43?l8g?Vfh&E!4IHQZv)#zp<8c9a7(ZlF%^fCJ4 zd2fFs)fi|DHij6(jAN$L%r*1Pf~uU+*kr)RjRqcdRTm-D<_&WkTE(0CyTpODA&q0S z15viG`3RHa7PVH?IQkZP6kNrBi80XQq;wl4O>H{7s;z5`qvzt;zL0Nvi#)|qZ=@E& z^%c0vwnDZ?AXSbMdq`=Lc3Ud<3f>Q^cr~m=gK8QZ^k4K}jWi?O7~#P?0KCuXyY=Vs zc8o~8zYTT#@!ygt-$-D{*wN({)&D^KdZlrC+)B4uj}9H-x-~Z zUIz95pc?BGZ4&j?!$|eehH*nbq#xFg=tuQqcxrz_KdGP6PwSuQpBp`m0m!W(b4x9n z`9it}^G6M*Voh5;R@DWcFaBt(dKK<7aB2GcM$j!aNOf5kb-;0@GNn@I7{_RgqM3(6 zsn@C)G2(HIa_;iiAmN9V^K7iY=diilp9ki;z+rZ<8wvmi={%BT#`-rpbJiEqy z%tt+>17Y15+>Ly}zD1~km3j{QjD1tZLG^2ak7JaO<0iGqb}X3tvYYG%yT&fFbL=?V z!?xi5UPdo**K`6M%q0mdNXZexT|1b0u=+bFb& zJf@-V4rc~xawJ+wCj3o|rp(ko*T2($*MHT2hWj(r^>1iLKf!$(HGfdwr7wf~6xzW7 z%s)%vK8aSbAG6V;aGyXM*@wAl3EaogVqU;Z^$6U@(0=w}-olPs)cz}Id(Y~N;68%3 z^*rXZg>WB6OWTbZ?qOy@J9!bUbTh`KVWh1V7JLM1#GbG<)%0rgXt zYKTgl*ZTi(?tiSt9{#ERFU%(^+%t-pO)R&}Cx1VF)9CFUyQ@d;zl__MkN&IKNW(ZK z*B`-RF2}B7^swIO9Xk5QXzZ}gz@G)}KL_i^3jBGCHHr^<&T{nQUHJ1uA3BTK^G)=m z0JQyUSS@~rzgp-q-(VNL5BI?iTw2Z45v!THVr5WItQ4A}jVxf(#3*_n=EciwF6PGf z*n|3q`iE>jX34MFL;9ZvXNxd9`m&{%X zH7*cWh|tQ)pgruM)JHUC3G8bL=lSe)+|LJMHKJw{9)+hToc)9{_D9_$VI38VrwE~9 z-)m#PQGNsbuy*9_ba3&``bh9@*PFv_(Ocl!gRbnd2niF{4s^YFQM-h2f9yElZSFDm znlG69%>Cv8^PqV|yuhnYXZg99wUJ^Jk0GTN^!TM)|->^#cQz-t4FZFPrN>3T3|j#upR;11EK>q1?&dh z9k4!NwV=xb76)widP}$|9PoPG>lNkC@C>hV=LArA!zw>y{MSL2bPIh_kBV<7;wuNOQeYu4FJQUXG+-i742%P^fzcvgvaHgOk3m3RAO+|SbONG) zaJOwuf1Otf=muadumV^@bw#i&AXshyE?|pg zfmaIX24F3)0$38z(?2R;zyC>KSHO1vla@J_3i4Z~TFS_e^b0{noZ{If_+=Qmr&Wbl zhf%mh4#|=Z%UBOxC?}zFR{+Js21-|@DdpVpZavCAk+$3I$wTh`{>4JC%8zA?WmG^< z%P?R7&>Khsx&pC4BoHd{Pi5c%OB>kJ#$p8;1H*s;KyM%k=nBLFkw7R=7q$li-hgJA zWBvjB1bi#3T=RPnp(`_J2kGP>ogAc-gLHC`P7dUAAW!W;!Ho9bWk#De zpdC;G!72(~) zHB?yALfFuOuI!?;Q0VC;Y-leugbEGkg_M`L_NOa*Rk-5>PglY7i3n*VuJ?#*eUXZv zNTrdumWfmvh*TnlwHpOzU7@GG&~uPN*moj4MqCGpd9QC}EJ~&+t=e&=nXtW>-{y-s zElz|)3-@G7rSiD8AJ;dveYp14cH_EAdjZ!W!u^AYEf&`R?RkXn6S4152=f-)JsSjPrMRvjeikJ71L(>p3EQ3$QoDpyHzBoH@E;e~ zp(5m8g6CIp%@ZN}MeJANI)g%3ZHi^B=*r7UGixV8Vuj`df+v9ZS(K1^Mc9)qcqR&| z!D4l=MU-0yx-a5A#OfeHgme>Y1TQUG>yF#X1noQKr+u$|&kkrmYQM3A+8^2nBxKl)$-c;Xb?e9`dR0Jq=MEyLQO*UEU}rLg8eL*=f{L$waKuI11i zT*bE-{eQ9PZ(2-Nm2a6BvR%qhe#uR=%4Xv?b1^~0kq(07^Q3NBil_3N4?C&c78^eR z*CEr~%rQPwvXln`=>au90!7&vR}}q7)B)m*w~W_~SB#U2w>sVG^p+ckjTinB?`d0( zvFn!l7IDV2NN=OD&RFS&x!PEYu!RcqK$qf8-87;emO(%8(l%=0EsUYx6#A-pP;%QGnhGPyl*DL4~RRW zVi6K1^xK8|4slJ>@PwQX7J9mikT#N2ghUJeXb}=Bu0!aGI~=ktRM^%^gm)67U~hR9 zu}y`$sc_#R!SBCh@Z$^^XwVM1+qP*HFP9DcpAn z9$diFLWD$%YkPSW;RV7yTDZFlcRO)yBSPYZws;YeB0}yaH%>^9&UWG&DzAbk6!R+H zZt(gXZ`gSw-UVNb=*D}nJ9#hOhuL^PKAhQkIv>rt@=QD>OyY&SgbmGNgXYHeL_F@Ao=NtJ>Hj6*U_ppWh1%8?>;VeuyF|(w&2RkUIky zQ57%ahTP(J%P_o+<~JXu{NLP4HC<`a_2!%6dPrO^h}a8oYxpt-eI2lNQ@c$FgH%Y>30O`>HqPc*dqU8Rs&XhkK5}kDd-!Vt*>F30BJ7`{5qQlSa0kK~ z)PURdQaw`MDW7B?#V7wWRQ`tc0I+Oe1~i2{nMyssj+6@^4_f zEuPw~8{^<6ke;}|fnCKD5BG4BpWtp||A1EB^pN*#m+to28}T)P?5(NH+&26ZO8E<} z0kH_{S_5uxyHvOC{F03nPvb?Pa+8gl00-K`_n`RFP$l_^#|cpUJP$Cqr+dI3?ht_5 zp(pNvpAZBzS8>!%iHG_cwcWNrV}$_FI{-UCJmCuD9t}{s6i?&HT>!QBPCz@rJ(i$u zDu0Kc(rKaGwc+*!C@soUYoHB4WjY9;bcw&If)6OQ-JyUxZ z@9wi?C*^}cyaZ~41hSR%Qdk{DdxF}PpKK&h8N0_+w;j|6Nsd4^yFq2(E=QuIYaT#$ zkZnQA9RW)Dqj3jiCB_~KqjZR;v!VuQ4B!n=oC&0(Jbwix{ZyY+hsi)6pcq)|0pdyZ z0Ocnd@NDmQ!#_@e>?gR}shf}5ZamOcp&2N(d#an!0NG9;{^H}1_1Aw~#DzAPrm)%9}ecG*(i%4FiaW z+7-#U^FrwmD19nB%8L^qyQpmusLxZGjR)N2LTM2%*+%6~pfV=A2<|fO3b(ub+;K!H zeTs8~bgfl>stf9GWK$C01CSk59wtC-fOJr~Qrjb&Y6GEw1)wpOpeI0Sb^u6U9zbQ@9H8)i0L78J z3?Myi0qTpC2kHX>0P#R)D0Z?vKj-u>x z;l2V;*`l08`q7|WfMTzC8fDMV%IbQX3&qoec+UKpH@GmIY9p1wg(pT z)K;j?WCApA(mICX@_@d8yWD8(C%#dDn~(H?_vSUwTIgH913v)N$8Q3E0@r~zfKLGQ zb+N8OKM{O4KuHE*j{q+MD}hIWCBQ0RDXMCw$a*Fs`ukz%Jlj z;8lS7|7QyB_!00^`E3Ax2F?H!N9`n1;R{gWq44(rln*NhmH{-@J`LOlVC)rfS3&zAZOi@~okRHq@Rbg&F#SKz82udhW$OpX)TvE85?*hUJl#YPsGX~`zhd3Jh z+~X9bHy)ri)g2fD&|G;ZP!n%WeAIs115^)Xs_l_1trH03rg0fmtUalH6aD~t11e6; z`{afWwpW45h2+zLIzTW$@ub&Ho50;n`JE~aiYFaz8{BTUtt4BMU7ofO&)=m_aqe^} z&F|r-GHW2|U;MB`grQz&j;H!@Q@7jGUtNci_rK%EbI3F;K+2K3CfQrUsr`%g0^0n) zhLHO=JT>WX%ekrGNkm#ShPd^?%?%0n801MQ&h7VfyTgdG-PJtqaN?zKH{9Vh@p{I& zZSmx*vem=pTk_xz{~u6y+C*=+)g9-~i`#!YpQo+F^LPB7e4cK%-rK3Tr-+4JfLkx= zcJtL#R_?feM{m!Ao1elx)9|!Y<%(1y^5d5$2~VbP#8 zZqe8g4$yc&{f6e!dcbg?DL`Ws%^yJJPEfipq5kUvFb)X%JSf)fLI=r@^uU{N@v(;B5kIumMz_#7Fb&BH)Gx*1^3VAW9(p zB#-_rc&Uz1Uczp&<#T}aTm#6S<(RAJJ;U!PWsIvgzs7TP@vriE>>tLHcj>FXnr9J+ zvs>7a+={)-b@n25J8$C#_P1r>+scc%1*enSus>NP%jRC(8|R%r$Np|#>{xShKkVdx ziM_%D@D_pg^52EDf8n*nx3AmNnLZZE>*3o_^>Om!9Q%#E#vAa4yb%lIjafKv!aDG# zycuuK?_eKc|62sN@)o=$55dm!NcK8!g&lCY>^yJH+ptIYoookl;jH!7*i-D_ZTVez zpAya6vrpLXJd}sA7<_@M1Gn)A?3BKMoq88>`ur_!=TY3jqj?NYoOj}}yd%C+)tPl> zaqLST#}43)OBdc1r$M^$?mV6+@I;=3-PU14c*`?}XR(iQdVV0!=1!i&bMejh$v8tY zh3Bz*adK!XAIHbzZB9PB4_ri>C0Y{9Zl{C+A1-`}lM|gIC}r{wVA*oQd~Cv)JAIe)caun?Jzk z@VR^*e~=wz$FMVTEuYUH!WX_C<_q~EzL-D4m+(jVQof8Y=a1pt)8l+4U&UARHGD09 zg0JIG;ed;@U80@@;%O-@#pcC*Os)RJ-}}d=KA?H&y%ietv)- zG^EddL{5-$FFY>qe+x!xL zhhOIJ@+gE!T;npc_q38*ECJn49(OmnwRFS`DnhHpXQJCL7*0-)zYw8SF5Ad z)#_>WwFX*4t&!GPYoayPnrY3oJ2WfapS9FNv{qVct&Mi4)>gYqYp1o>LbWh0TQW?jn?kbGPF!>jFzQkYfddk%hkqedD=K_ zyq2#OXoXslR;*3XO0-g~Oe@zWYLm3d+7xZ7cCR)~yHA_0&Cn{enc6Jver>k)fHntT z)Sst4h%fFxq%F`M))s1uw8h#Z+7j(iZK<|QTdqB(t$-o;7d_q40p``QQEhuTNl$J!^_r`k2`U)pEd=h_$Am)cj_*V=XM8|_=|JM9L> z$RDtu<|plE?HBD=?Kkar?5p`xyNR#XF`et0uIq+w>J}XzxYvDjU)@jl*8}uGJxH&m z*VcpeI(l8bo?c&Xpf}VT>5cU!dQ-ib-dw*!x9TnQmU@WZN^h;V(eKpT>UZhw^!9qF z9;S!u9dw%>p-1X=JxX`z(Rz#?t9R5p>7DgBy^G#e@1}RxJ8Ed>qGRR`Y?UCo`$dNjnGHxqx8G=(fU1lhMuX9(X;ey z-Kpp3x%yZ=Pamg`*YouPy-+XGi}eY5iC(Ig>E-%FeUd&|pQ2CI@71U2_vzF18G402 zv&K_vHJv@hIa{1+dR$+ruhLiRYw)GNC-imtllpr7DSd+Hr90N zu%>g3Z|HC8=k*KvMg1-PZT%9RCeq*4ui!kM4aLM7oA(YtWyBK zFmeK4CuxQ=GF!3F>nWVis*Q8q>v5LI7pK94jTn}I69gS`Hn6ie7d0C1<-4-(IH~0? zPU3bq;*A7#9ybN24e3;2Z|pJOW%OaSs?O#Pz{#o2IKg0-7~o-f8ZzvVTZF=o%OOrwy!fHRF&dDa+b#+Tuw zEtF&@RvpJm2!<1u4}@i=xs ztuj^{YmBwV6WEvYq_N(3%Gh8$jom-b;P*y08(WO6#|L4~!3ukBpCvPmE95KI5A4FXJ=gbK?u+ zOXDl!Yva1{jq$DVopHnX-uS`z(fG;u+4#ly)%eZ$-T1@!)3|9=Vt*SqHB&bY)5LB! zFVh>pfZ}WVVeeaj8E6KXwanVs7gxuuYt}RCn+>p+u94Z;Y+^Pwn_(~79j4W6VYW0w zut_P&3R7H#=Z&R)iU8+VL$12ll$fn6YL@vy<5w``5adUCnN0 zcQYP){SwV2Guccrdtm2ZFSEDV$LwqN!#==Na{$hg4>AX1huu(fm^s``Gt;quaHKiP zyxSaY-h=&xndTTX%gn~fyd0d%8*Ap7Z?#Jn``_0)b9^c40e%mRq1?I!Fn*{sL9x<2H zc;0@EIB~!3uP6HPouhYg0^=&Xgp+z#@a3WRapLy__8xmjo%P>lZl^seI6=6}e9qi$ zK7ZQ@yF=z-dCJZ_PG1GUcTP{6FPblzFPpEJXRv$wRr8$rn)y1ue)^_)-n?L5Gz!b} z^DXgXN}Lm&UhxH)StUh-{dMbuVke)S!(s`QLW9tM^;fmL8jE_+oMXx=s6~{ z#II*Hqt>gprI$3amr~bDsHy- zgFQ?f>=ADaL2fNWgrHY>VV*4_I#STchaDRdk5#c!o7-#2ly#S>?9NmTEYm4%&8jxXH>=733%>D04a_NoEl%O{bymfDIh8u6 zYFW9}YW#CO3fMQdO2!yVEo*Ewqa{yh_sJ8s;!|W=-Z-y3r8rNuu{=?-vQF$Ve&ede z{l|Nx<3B#P#OW-|&n(Q&%d+H)yjk*PgUI*bGxA}NrBLR*Fq3{6tE8xStkY5`c?&&w zbth`JNH&6^TN*);G}5ogqY>f@Qs}Uj5~-)eLyx6YCRgfVd#OjfQAUlhObDt*XjhFe z!BQ^MFTW-Ia#cv>su7lpMp!Nz;Ut;br0UdsCRH`UNwN`6sgATvk&SSw5c8c{RbPQw zBE;R00%f69d5dn1m3gd+mD=21%1xv#CPn1R5t*oHyrRj9#tIr0VN*0hQQ0M&}!mG(qMC~A{-MnxnjIk#W&M@YM)B4oaz zB2~JPD&0tBccj~H#UH8cja2qVD!U?;osr7ENVk1%{VM%PWnZMS%jV{D+wCp~cRI>m zo6>Jn>Dg2|Hl-)hZJ$cdrtFVa`BFn;RJ76?t@K4JJ<%%tXk}-#(ifxTW0bxaB_E^Y zW0ZVH72i?CcU18m-SJ9)N2R}`(jTw%$1C}GDd%vPbG(v|SMu>nK3>TuDER~>pPLp3ZC#m$4RQgFO{Ul{~ zl1ev8rJJnel9gPtDz{`MpRDAQm3*?2Pge3NNb zs+~A&Qr@B3nM1WNhfS5A>Q4@vl2`rQ5h2reL@4+KS|l0q|!}N=_M<Anmsac8zvSQc1(Q9#ie=qMTN!)MCr+h9x@g#qZjV{%s!dL#hJLN zDj1WUsr4(@`eTDd9&U*8=+03akf#qCTVxK-%Pq*%hh&y}4V9|&0b}#@L|{N^o=hgb zqpwK6tf;W4)L*42Jl;rAID9BU8L#w9kJ2eTzNrPyT*XA}?loqRVsiUsqG}OfSBn6< zn*8l*lC!Hx&aNgoyPD+eYLc_7&S6)Rq+K;~yDD6}n!N0)(b-j_v#ZI=t{R!|W27i$Pt`I3t^_*K5-BpQ(-Cs9x( zFDFa*RXTPhFDFZcD|y*&B2whw2CB+g4tfzOauNhpIj@8(nalZ#%2ry5E_H=#muEa$jo-;mt}f6)etll z5jsVdi6JPDhM@6c2+9{jknGn9iCST~HaX9$NRC5#$=D(plNf21iovK1gAu;kOXHee zjDaW%fY&Ud@xzDo%E5(P?m?rrr$7xA~=nGD&!RMQYM1GA>zEW`)Na znH3H-V##U6u9h2iwal=`h#I!pV?@b}O|N#fOt-6P)~=>myIQ8()%0q2w+y*F$4Dje0VNLy2F(|dHaNLr8idb#VWp7m42+^k9G5@^kY@} zu_}GJe8R}4^vG!*ex*lF_wXw{a@vQzDqhwbMlvNYr+N5QyqxagSMqYX2fwmIF4Le- z#mnU;Mm`lUZ}Z?+`s8vABb!REqsq6utwp%Xx4gZDU*%ig=EAS?EpHbw!YMuS_62^G zj=X(=U)d#ZSKwE6$=egyt^BgxVq{c)+3w(1_Q@F)er2C*pBN!k`tmjeewDtw4S`>! zFK<7Po*H@OG8`kHikHhtjFd{Byq$ty>65o%@GE|YDi?X1fpB+wR3n@0f6%Y;BUcFU ztNh5@Vfa;kN}pVb zV1!la$=eF}RlK~tfM2C2myPf%ee!k#`rQ3MF7NFz@jmpi@$j;u(j0HRB#@4>NixK@ zY%CstiZGv?qVf{u%A2V8O7kX*Am38FZYY#aXI}2uGRat&rxeOGD51cr)WXR{gJNWa zobEtH;ev`xfQm{36`24P#)FDR0V<|J(0Cu1E)pK=FHDy1QokISs@(p`6g7iC)TQKki-%(QR{wIb9$`vOH~S zS#4AIm^Sqc%O>wZ@yu1$k4+9<@XPwK$w2{rSwA+}X(Me>@d4$9+0GI?pDuA`kI4^m zPR_z~pItPmFr0q&q4tDwXK7hpQDId`5HdFg0(rTzRjGW6NfCC6#*1{6KPEYF44#xb z^PM?mLSc~bmWY-}3DhG0cxPD^Lv0mM%@SCgS%OFXRT_l1#%QYBAdhJBdq`8Yct%%O zQ-s4eCoi`gMSzz~VmV-oPx5mXkXM-P=PaytkDY=y2(o)&Rw0Vpy~_=wo<%S0ObC`K$u8qYu4+6k-8cdc}BEDFsg@6pjt z#i9;s6qUDU@W%(>{cL_wZXSL-1?3%(SD2Gmm{&GMRIo3S%*>ZVOhH~@d8r&%iu09A z#Z$ZTmGR{C5%J`zh2?Zf(PXKGY9c5TFNx$UFOhk_?7WG2*>VV1%81P$FS|046~R+m zd8)Nk8%fOY$+8gKKKahlQsD|DSG7=3btXl$GFWIRD9UxC|6*rJRTHTNPubX_@>0B)lgW#`lVBjZWTBAP9|e(Jo+VqBe+g~`P~g(e z(sJCKP?ZUm!0_gi;|LWe9aA6%s8&#uSx}m9FyQ zY{f0t3y~4>4h~dJU2?qr2RjC9B)C#YDWfy!CWA(x!+%UQ`Gm!9y;&h3y(sYr*s)d5v}a%qI*l<|qlK6!-` zJyVvC1)xb~M=la09deO~aIu&JRoRn|o8VW)Ef@JnK^0egGA0C!$Jr$qJ!IzO;Ti$%s_1KO-G-{f>BLio6ej9wjI5*CHKqy$Y%dO)aq; zYRTo0>(@w!TvvlCljRc%_?3S7X6T3P}z!4-bthRD7|u}4!_;2tRycp7lVnQ7y$$=DU&o`%#6^ZawP98 z;8*s@I~>%cyQWnQ$va2Xq$+9opgGbZ@1)QIR3npjwD7BDET1vLuk4gBbRef1X_S&v zlaxc=38Dold*m}oq^soB;?AK~296k2(`vEjkk8m4ugXcjpod?Tt9(*`^i{lkRtWo4 zdh*T>er2D$6GeHr{i;0VGd<{0>B~FFNQb;b2UUFXNdx#)iW25d;tc#RQbz0W7y^PtMZZ09Fd+Xe|bj>zp799Y!Z4@I`Ww-%Ew)=DqKEe zgnnhGe6|WcrB^<2fM2yEc_$6MYGja4C?K!uS3ZdVf0BQG(Nye0kqZFPIie$NKA1K! zm0=Z)1@f}>vF`-iXU32fHoJP; zY*%lWl{E+#jmllPT{b zs(0%Me}Mb3r|<<Xo!T%3;b!s^Tg&xQ(lNl!`}!qO3v1z<@^^q66X4nexk77=9HlXJh!4oSc>MNJsI@ z9Sn#U9U0+@Prl?qxQ*(A!i7oFlOk`*ROKXkiE_`H;;*RSq{w8OoPj}wdQj1nK<(mO z75&Zte%~SV7f^Dyf)B@r;McKRoBZ_KZ7h_&b%uZX0bJ-e-3WAA%c=k!*dr~VUjn7C z^#S@-&=3jydIsqeFsz{T6+gh}2iiu$%mk&c{sH>?pg0SMlUhu_4oYXm0R0V6`W88W z-|vBO68c-9VG@)T@heIA+BtrOiQ$((1@sR=ajp#~te8Fvw3CE>5wx>}{sCy5gpml^ zMZ$;$#koF$LGj%r^edp19kiC5s$VMthoFf$JQMnLEZh*ot(1E}pAPu0e9};i|5$CcbvOtF_j0Ghb z{B9b4`BciJgH8eHyLN;bz(@)GE6`CAI(@^C^aA*e6o%hw5irMt-Xo!Z44NTft^}pJ zC(HxJ0A$NjAX{M-s8ity&>SEaz}ZTipOe@Dny0V}bezIrP)eI{49HhF30eRY0q@C znuJN^bf1L&CFpbs{E`e~>m-aupifHZ?|`nCFq(rtC1Ifd(C^q1j5|P|mcZ}QFt$;` zq3uH*d_^c1t^s%U{W1yk-)DbF`VcSFuXvYl`xV)w@Ki{3ByTD0h8*D>H{!W zgSsT}n>Gxm0R?b|gW)%N7*2OE{bkVI68L>7h7%nE2KA3U5+=2ay%PF)&=(}k<)Hf{ z^p`;QOW;%|!!HyF7*ys5CG<6*ha_}rdxs_TwV+2N@H;<@9aZoFrFKIwf#V8P4kr`> zK&d?uD6LZp{-CE70zqF?Alt}RLQ8=1PiP8ITRa0$-JMl%fWE3=13d@421Ej{D>MXs zL!lQawPQjmK;=c~16)w(2YOLq2g@-^tR(J&T6NLq!pDHW{y{51P^j`|phCfr- z2>Q9gI#B9wls@&9uN0{5e66q>^g8ejK<(~Z1!}|JDNq^TP@r=9UV+N@2L&qE9~G!v zeo~+^{#n9^2K_~W%JNqQ;{8p5()k_u10dahN^k@8rUI2~r36kvi=)%j6=@E~xnBXC z1rcMA7>9^Qm%v#RM!JMPKTx!90!}qCp$okD^)1HH)(LcOo5n9OhSdZ0k-%?jGn};+ zpx=1qqzk}lD2B7w0yw+IZ~|KZzoX4~palJ9EY4&L&@WQs7eoYbx{cxYE(PeftU1{M z(3*nABQYjD16o%Cr_LBoaSPBGiPPKyv^Ejnt!)6=r{P9FA<%jRXS@Yy{KHvq0i07~ zIP)z)<123}LF*AtX#h0-apD7Ly@HeB0yIW)s|2lAIHe8HxX97Z3ACo+lm~#uPMjYX zp!ExHEkR=>PLm7J8V9G!1!x@QWCwur6>kdjX0F9S8`z}ChE}VfEpfQz`AD}fCCmDdo zQ%?T>N7{FQ$5mWy&&<7dS6$j&tya=X+IrPx*+h_4N{DyYVgs z(#L#?0@++xb1y-96D#i}$adq?6-XarExrWVYn<)@NbmAl3S^gIjlKlwR^FpPb{JOe zOOWow%6$p4#jt)~f^;XJtw8n~A5b9u%ISW9Y&5L$mmr3)E0GOYWTAU%p5o+a2q z&;<&lYdL)f$iCxr4?y~uFIFIX4f_R1kZ$Em709OJQIOu{bR9r;A78FOx*2;5NRSOv0SVHn*mXdHY(Tz7f%G(At3Wm#_92iUU5ot)B*+%z^cz5W8v7GS zkUdBzED=agV_$0tvi11!3Z#Rvhk*pyblAy2!2sQ)K=vKqtiX1FZiylhbgKf^12XJ- zppXno_W;-xfng7Ag(T2R6!ZY-e--F#IzFVpc7jF}$kxa1*%H)Oi);n5wMC8s^^GD| zfoyBxR-ismp#2iaKEk;X5(eHc9#Swa0)0fm_$}yD3dU~GrxlD#L7!1DE(U#B!Po`* zih{8h^zRDBRiKnt!1x8|YYN8Ipsyqpj^QKzcitM)t5}e1q9;)P+h?| z2h^fqfUlZW!8j8%LBTi!6uc%F=Ypa>2*%l-VP!8jij`jTLr2imNF69Jjlrht6mpK~4$c6Hv%1K@5OGP6^^u zP{^tV88xbHV z62Aa39hCS8=uS|o8}M7q0Hv}4q8pUz<3fDy0li2;ya#$Qa1GKSbJ}&l&G_5_dJAwX zKKFs%2Hb(qAAu5&0C5O(zk>Jxl=4S1_Xp75D~NYN={q3a2YpsSdUPe}jGse1Yd22K`b&ECr=^6R1tl z`v8OLpWXq8WuWw2f)3ELP!4evD3$rYfB{fB@ocdIb&U2${vX*l*mXdH`a>P>AdsDe zeF!9|AJ%a{f%<3N0wmyb8)zbsgwJ8nWCiMb^%MoNYxGnFvVrt81+qJEQnLiv4SELP zz;kjzGXW<)dqJ~+9DELf(r*CSO}ZNh;Bz@B{bu5G4QQbP*(~H&6UdIy>D?vxTn|e3 zBVS~j=yX3oHj7@NK(>ZH3aCW-WYEz-2%oP5tpbpDelsY&5AWf(gVJ*WemCfN06gRO zg3`0VC;k}dBnAFBXuAUc4Jh#o@8M5?b^@pe{xs+m1^yK1R0aM=&}qPQTmxCwX8<$t z`Bu?s{>jKE(6U93R1mA*uQzXZBef$S`OnF2>!)Q?gin@eA= zz+VPkp+NQ*POg^VuYj&nAUjN7t-${ddb9%BW%?Qg{wnBN1+vkwbA$v(8`jq=kiDiK zqrhJW-Jn3Wn|`bUe*^S51+wF?*MtOr6Z8ZHvg!1V3j8h5O$ub+>6;b!+o0ztkZ+)$ ztHAF9Jx_sO1A4xKAo;yOfnN)Hp@Jv_y-0!l8~tJheiP^=3S>*_I~Djlpt}^vrqnN0 z;0Hl3Qy?2t->tw8f$mWtTT|bwz~2SET!Cy){R##C9_W<{WQXd%P~iUny-I;>QvGTL z{yykF1x|8AWd`^Mpj1wP>{R_a1^yxEFM;c!(?)~dpg=aPPI3?Mk3gyX0NJqm%?kWu z&|4J9p4D$v;Qs=>O@ZuM{dNWZ3FsXPWasL4D)3K1_bZUit5e$m_-CM0e*oFP`rQir zb5Lqa_ae_@LGM!_yI8+pfqwz|fCAab`hyDmOVEcD$X?bTR^VTOKB7SOvi_(7{|1!W zCqTBd{+I&)7WCJ^Z;-wo^l=6L9VqD_fb3}fNd^8N(BCSMJ*_{bKsK!Yv;x`J`tKCT zp4FdGAX{7iy#m>``X3a??$)1GAUjw8qXOCB`g00`fj+Om{|)*l1+vZcKPw0h`WFSV z)Abh=1a8*<3cLtDSAo8yK=!--HwB@AzN|pDy#9)U&_VyMKz6Hkm= z$)N8m@KZrQP!K7gA1d(EK>w*AQb9jb;HQI98vsNa=)V;B8K5NVfJg@=`3Crzpd{0P z$N(id2KZT^B(H$5fs%{@{A^H?OF-B`zgFPqfRbDS!U0OM2=H@3sg41W2}-gE@bf@P z{s7?wC7A>G`Jg0cfXD(R*#h_lpd?R#aKTQ33=#N+ppX>;`BDbtgTOBWg&YvbuQE{g z1b#6nZX}S8WuT4;n+2qF&@bw=R3Kv7Qwkq?TxA@EBPzZ#VC z2Z&-&$`io%fl@vIQ36Wu1}OGm(E9)m`8DVr0L2CjdM<#IrkFv`0w^wM(EWg@1T9sd zIHFOeASyu16)4td(04#k+9(BzGa7V1AdptDLxF7qJz0Ts&{GuH@t~(FP<*%GGzGR1 z^mGMW&I&$PU?+iop}-+SU=#Czk}vCl9C@hTSA(L?J*Yd)3yM5>h|kE2hrXli8p`Y; zP(IHB9SzhXeIn>M092q(Jz<~?>C-_wfT>8o5p)L7kMsf1D8znz9#jwqK<6olyFeEx zh`T`-0?TplO3)R+Iz00M(DlGENJkkw8-Qc+8Fl744gjAt%Hs(Dc%!w0f~TI%_}mM+ z1=xzui$G5Vw&63L<)Jd4iO()jD(8iG7u|Oeup8+$pnHI;k-igjAAs_apX|8_xD)A5 zfKu5|CJk-IL*)T9@Yh4-0W`E3&piqnWW#fxf@TA~UqSPLQaK;QGf^JTL%_rMjBs{*Bb16m05Nd<~Mc%D+w#)3Yr zpf!U24tNInIRo_fz_a*FeEOq;LFIT(!9W>2&np-xgXd4c3wT#2=t~OPWYE7UXwyJn z242B4uLu1*@G3rE2l~2#LA-fG!64qesbCOq-U1Hd`Ab0$0q^2-Kj?c3+7i%zC}<>8 z?*kv;`BV=d0?+}1`1X;4K)rfC2L6S%O8ovr!63eVu0Syq&ld_5Q}KMMK=BmM*9sJK z@qDA8Z3q1p_&45lJLq@7fAN`~Iiz4vT}2cO&M@@XNay>3;y-qCozocdG*VtKJh8$nW)T z15U#ApMrvq1Wx7K0i2EWM?ud~AfMHHt^)bK-t!d5m-e0yT!8CQ|K8gabSfX}o1ml2 z-Uk)P*Y`f8KrsL>y#t_FfDb$;7%M@u6pU4%coxAp3Uri$u>!Ok=)rrh0PO|lzwse|;Y+7^o}1P@p)0KV5b$idpyQm6epr~VlaV%(&f^iIJv4R2q2aq>{u>tfs1><>d;ECy+m3Y6=G4ZKf#bPl2W?kWFSrL3W-AJ`>26GLe4*`8_81O(5IO1iuMn zFPaVovct?w1@f6p`VJUqdnP>#Fu*I5@&}OpWM(UnFJ+>>2nP6WqP_@5GpJjE>@X9$ zf4agdS06X*~%v66)-68?F!^Gm?SHJLDy1U0tV%aWDX!3*`#*>WS5&%PXO7z zCdmXq{)S0;1q|Xh)i*%Che@&p=X~51YHU&L;8E5M=9uUfi4GDApH>NN(J)o%~cBIKbxx+$hS6+Rv@3=JVt?h3v+`4 z`5Wf33gnBJ$0-;jv&SnKB%3EF7$mbB6^t>Un-s`rGdC-cZ)R>$Fmge+Dv(cTo~U4i zK({FvB=09F7&)NZ6^sJV9SRg*G*1T3LfOv%JzIg|g62626fZQ-RiHScd7c8r7tQk( zDDG&|vjB=mnincioYIU!-;K}o8}6gno=MLFD6VPJJFdp(J)rvl{7$h7^A_M%eEuBt zHsE%AeggCk1&V2zcPdcq%DhW~;sxeI3i@HtM-(V_U_Pl}Fwm!fr}50kK%Y^dxSjcX z1&W=Se^AhG1bt3HzZdj*1&T+Se*#{>b8ZHGQGsHI=1TzNo#Klo$u8uSVu9vsz?=B| zFz8#r+xYw-D77)jAjK@qcNHj>Vg3X7C$9Mt^b-Y&b()_7Um*S8p!C}}_`DxecfAV6 zEYNzO0nh9P4J#OZpv?+KKj=aQV>aj#1!Dko8Gv?T%mGDPC|r%tb1}8%8U_7y#>xx@ z{WMSuU_~BIVGPA$Z)v#xcbF=LGW-W+;xpeR>XL&C2?D6dL+~nEs zxzF>I=TDxOJg<8`_k8WOf^T-O%bVx*O1{-a`F5Oli+6|j9Lcu_NARuN*9*Q~JfkOoc|8%hD;2zwM)_QOPaKgb2_`Bjz-=UsE zt%vI0>3FB}oqD<+v=x8-fcKr8cQW6G-oWQIZ?{R>{`QST8G94$_D${0Y53dqX4{+1 zZ#KLEErO|`ugG6h@#&E_$G(~KX4ad|H@o>l7`o6O%?GSG*hq4FwV27ejc zxEYN}@|5>1=Eq73g1^GJipXHYp=w$rzkb?Tv za!)ihM(?vcVm%S+2_?l}^h~jT)?2K2vh@xto@|ZFOzc;p)<@#sG@LW=hW2;et4}o+ z;go@YX`g6+)~9PP=?rHNyp6L6-h$2=r#EP?84H=tGMJ6Im>VumAuD5}pcTin39N;+ zvQE~=X0rh{k1b&<*s<(5wwY~V+qI8ys=>#`YVDwLIXjE(VwbWj*wu{o<-e2N#eT(p z!=7ZnXMbRSVQ)Z5zs=rdAFta(`51XcUvRP~mTgg_lquEy0&;H5IXJ@l>*~RQU zb`iUT?O`{tee62+OZFhUhtatLSF%52KJ*LhMfMtdNv~mlXJ50=*_Z4quCq6}g(a~I zn1!9glG%mK&Msq4wwGnG%bA1iW;yH^ESFuy0_;YX&#qxUc0CKSn^*z6mKC$xSP{FG zm9jfn3A>$DumhOuxgYa7A4SjfFdM_}Wwq=PR>kf{FZB?#?XNMH@^SVAYh=G=VfHke z$evQ*&Ox`o68QdL3WVM zXYa8^>;txtz0Ve~f3Tz2Cu|w}7hB3cW*gXl*gEztTgAR$$FT3%TJ{ZF&;HF$zznw& zxxu!v9Xyep!jsv_Jc*^VovfR^$`-Q^*>d(N_N%|h*lk>D>@xOZkNRI2R~uItXJIG& zld+@yImSuGnZ_B$xyISXcIcf}X-TL3BP4ojCMYoIms|&JI%N2GKb}f5y2(2XM~NZ*g|hi+n0h5SoTF zgr?&Zp&2+wXeLe)nuW83x^bFN56%Yh|`AV;=G}GIB{q`&Kz2RQ->Df+@VGMLptvVXFGj_Q=LA>xlaGW$xff( zY^P883Y0k;^X; zZoX6G@m(UHUn&atWx~UE3oqXze0;C)^UFnmUm=40N@4O}h(dmqDB@R(V!lt5@Fh5V zX$?+V$`O^^B`SE9DCgN?6x+l(MrR^87mLAOw91op3Sk-2`8agy4V`VXNLwt=sd2SJ%U$oJ<00b_Ep{#rot(2oTOm&&qSJ`(H0Z2`U*lAq<8Us{vD$WdV$rYZJR;*a#(mmh zoCGt36JZWTPkNyoSdLTqsH80UGE7Z)1*yduB5gSB zqZB8FRN^#|(S{vomb@fSDR~1Yo4kN?O+LW6C9mUTlUH!U37u~8wmkRbMR}shTXeb! z&N}%RXPO+unIaW9TVyOw6R9%B;4~69P9t&Pe32TQOHzO{TQ0*%ExT}H%OyCu4Nfkx;i58t>?pPVO`D}*I<`b5Hzpxs+8&p>n2K-j}t*Rz8 zHp`i5u{tf$Pjn5BMCS6fk!N`ArRhmY>D7g~fqa+MlAJo;-w>)8+mM-_o=MPizdOs= zRQ27b{MpphaI6{s#%BsaSfcj;qiC=#~JRLUki1gL* z^c#?`;p@+jr{8o$`d~c$7D}fw7DVi-Fifshk1IND)ZqlQSb#Qb2==W_He5WrW@~sWWxN?7bWq8M2_=(tc#cs zd_Yz@O>r2o;BM4>21F&po?*w`=_nEXOHOdh+D9o|A*TdM!AWBVe$O{0eH41=_NrSy z3ua87cjukrK>G*nU$oyjnI9K95A~(;BINOJq=aGL?`8SP=`b`bs;xm5cz5rQ;?d&vV*~(fp)>NTvjw@Rpm9cE~@iFYy(jf z;y27KA2Xq6-tik2RCW3*rk>n;*0kwo&J>UM{B@x-cV2Q6^iP)OBqTZ2;xM0Z)U>VrjNO(u3+MzW(Pf8we+527|?pV9*N5&`Eir z%tf^jdD3gOql7H}*zYsO>g4+>+d~DlneGC!p|sr6v3?@>5k7)TohMGH^j3yalSgNi z+aF&Ki4DDjew4oYZU=c;~R;wq%IAIszbJr zU3L=?Z70BY+;#V9S52P0bKS-h24`M%)hx=vlY4l$nO6>7y7h`{*1X^R9dSUGR3mwD zQOS+#K2@9{%}jX|@U2LG5dFnjlLz1dm4pi^MbOMj!@x9P(r)08HUI&s*QQ7%kU{Ov z;d9FWY(A^+uNKuhMTkBAxmSm44F^>C|Va^jj#MIDeNwcN0rN21vI4_ znlhg?hQ}u*!XI#Qq2=PRI7}NsltSp*A}~Sni6nKqP7u6xB4+xQ%h`yP9k?;Cu&g|oszL_^hzT*}?6w&yk$U}Unc?146uR~d7 znKa@}8OkUOX?vlp%Tc<|WLY6GFkl&?{f413A_h>WQGu~Zoq$ZpS3_s{_-A|fMo!qf z7tY@5p})7aiNZ;EUh?5hILY<^Y{TNPnduOs{pTK2Ne z%+Ww%g0NVu1I%hA*+N|mz)t7TFVM);WjAWjFJ8zGj*wmO6BqL%Gz%#fsaZHU`a|8K zSnTAp_wF63S}u;)<4DCqbvkLdyu`yC5I&y?N6X_qUxcU zO|dzGe(G_}VGDxlodjv!w$P&D!miGaY0vV&yUWh=dg>|%XQG>-cjFxhM^c^DDajoR z0*y}uT@(2SIW=gkPe#t=>+!yo&=&MuKjN#f2#pKHT2PJ%DrO8L-C#Lk>O5rO#GF_Y z$@yzO(xH_~ghIJon0}RDv+wDWw$gdg>#grYT)L@gclPrSFUCv9pi^dXq}0 z9BdkKeKehG#$I-NWIszh!1)7_{rf{`*c>rS%!9_gsjNI)l9HU1XfZHMCY=JZur+8g z#2KZDjlZcZ)oIfcvdT=W&t@&E1%GN?R;O?!MK;%&^ZTwhe?N69PYKr5@&3pg%g1e# zy$sYU)WQK+S7_rIR@m}n%|l6|Z(L6XblkPH+p;ocSK|w4h1EF37&eeqBj3^y67@Y! z%W?fD3f(@dX!H8jTlk6vmq*?WQ?AF$?__FEn#=bWJ2#p>(xQXQtpCBz5g*=+aGX>LL-BH3_bHz20JPQAugk zJ(TWI5n58zEgb7YdxF>KkbIOzevy_H;vYAhx+UoLbrp`E0EYU!-mb#P%lV$&uH4?K z-%OPs#LDZ|Uh}whm1AS+wd=s+gmsxGMwV^#h-Ax!=STi={`reGdWOET$nsG+NRlxE z_A~sB#SAdci8I_Efj91BpLOleR30OP($p3uC!%e!K`K2oTmr;b8Flr>6nEGDEL>Va zz0nUZg(DHYvWq1ojJ(N(n`(}@$vT(>2dSw`y$sU}NSWe(GCj~v5)y`;)KL6Jo|qWB zk9w7#xocoxAe@~A9i8RN^5*5DoS8nG9sk)<;KSNoh;3w1aKl3O6WX5*VY#RgF1@g? z3VB=7m-W`mFV~xRa^%i{@5R-tCf1E_2-nv&iAR##+kV-8b7y-pUwp^?9cS#m?2Oa* z>^TkjJPypSfQCqdofkl_twTNNYzf3jP%uGu80eIIK|}}1{$|qqld%o7Gia`{Xk--^ zoH)>T=k{~w^>-~@G_9YnT6LI-1FKebwnbiP?Z#VH<1OWQOA5+W5iZ3@B}W%$EWztY z>(J})CTX~(rlvYmGyU*4y<~)^sazIglD7i?MF-@j&BckdL@aP*qv z*6k1n9*R8Q);D!BUIu%+TOg7Fo6gHh!$s&~q3CYKyWwXYQ0gu6T4_-~Jt^ z&Yd!I>9VQQ`Spt&k>`0)QZ@QCu{xv1)8VY0!~3XMpl9^C-(Y4>>_u5hfl^r?RT_>nuo@WwOgM z)v?WCXGM9oOE;{r3|K0B#uVxhqrs^l%rR(?75D>7cmYgb+1gQPivE$efKt1|rn*e5 z|JmyX!pXt%AWX|bTLQf4AR_Bv!LWrXV^kEb`k%RW+=@|01guW&{uvIgT4Sx$N5roB z5~uy|_({E-;@kDaH{!Fj_8y6*lZ^CAev&0u3E8nScQ~8;Lg|j7X)B`#ZXq=?A320j zXuPlgZ^D}RCvj94u26O$-JMSHRxlM00KXR{!1FXL0|)`ebOatZ9Q{p%e&iH`iu#%K zXx?|~sr{#%LQfY*{k8>TAM;y&+tr1f%N)w+(-J&I7}8RrcOSOPoV!^|IE?pd$@r5b z?IFb85s{CF95EoFv_85TtB?UGJju=6s$)QBLXHBIuMZe-0Y zB907QPMj_Vr->(i=wxNttJc_o)|i2GM~u^wH?aoj2xJDLehalgXgxoCfai#{DUzyRe!b52k!w#z>=Ksj2NzaG>bHnr*$1m)k+`Vf0tQjI<=vzGe4k3mjViuK0K3~cV zEQ%4mj7lfjQ0Xz*Q0XKaDm^9}DxG9QrAygB{t@O9b;y4*`OQgI9b8f!^pLg zo-=+e5^9nD_EisGDziV&sX)dAt^*m+N>lNdY3! zq6tq=VEN(P1jGu#)u_XU(nzLA{z7wsy^fH^XV3m%;r$QJ`G6<#n<6*xIT6|a$md8d z&|En5KT&t80?*c6AKBBW04F zE_1&f_agVvc(#12)R`*%ItP9x8J5>mq)DaU7)@6rLOYSJ#y;*?s;*bwE2X!}=ilK| z>BH>Y&f``l5i($#w@UA^~?YNVsfZ9~*0QRE_AUC1VdTe83&CphE5 zusC8XSTo%TcC~|zaCYSoo(xyDc2NL0raANT^9%FMqCg;6MCNCZjKLyl#=yPAuGVr~ zyh!?)7!OB0mvRUXHhwfo!_c*s`TV|`mQ6=o$>VDBh$r~ z@$@|-(=|st{qm9N;!r&ON=lb}OqYDDz)Z2uaC-nF@P4l8wRT~(lxGNG#ptv!XvEfH z)hyI6p)JygoiPY|sI?2`k#rs~5{VhYfnW)ZI_G_V)VZeiCmEXwu4{_3V{2;pZNuXU z9aFr?Kj1QWzna)w)b1*ak5X55<~a(#$8rzqf!o3K;sD97;_tY4`L7zu7fp|+UoH8f z9sUk|?gGeH8D;=twp`c#LU@VJ05>efegse&?8%TX3YF@e?jK7});kB(P34$8%9F4w z(GsW}#uM10)I>6J22-qvt+sZ-_`qN;ggQY z<3f#9jpbv@$5f6&?n{Gy1Wx>!>9AB7MYnv?)Wp!RG}lK;J1W5{rQL6q8r z>WlrJG5KjC_skQsvIn~z&idTkit^DVvpc){%2uYNPxM!ml~hieGOw@uxmpC6MujR` zP*;lgvW;9qGHUF^6lapvrKpok@iFFTy4e=x?Ax+kX_#k^0cKrvrot}haEIe*B9?}l zpvVBM0TmZ%MJ!v_`5^Tr%`~aPJ^Tw)Ykl-*uo}O@R!C1zcc`{iKe@+EFW)En9+iA>XgjoYzYs zoZxl&H!5O#_!}9qrM9E4Nr_)`>G!XZ<*>_gl(KMmLM~I(moIjW_-~vCd%u4T1+uDU>Lk zTjhqx2(1fo0BaFLEI_Jw>6UyqrqwaGZc?ampnLr4_V9{^`o(pN8d6)vOdrp)^qd)E z#!s6XZtoUnx>B1R1(VCe6P~=N=fsx!byGXmH?~bYDlnrU@_BJ_=|Erqf;G!fH$}+# zGVtBct~iLGwy}>3Xzhdpg6|)zgjldixTX!)xLAN4OLc=zCmSOVRzV?DBU(3pa_kJ3NDZzllW;gw|jKt{hXb~|c z+ES}AtD@TF3~9rhsTFP0I}_^WQmuviYi_-@XZm%hwB|`+)Sl=ay0vYFs<|z#JxQIB zfBOQm`baZQN9mHGMY1rgT^FvwNS#K01FS_FT*5Fol_(~2Xn8c92}0#^x$<3kj!e_^ zkpWag)pLYGksU@2;*iMYExk=eO=gj~VDyIV<5sjSJyt}nnmNVbc0Oss&e^W)arxd$ zw?VMZ-Q>#pXMa!kY}Aa*m(=7}D@~pR5>F>yt9T|(W+_iU<|g1G%PyyC`dpT zm%x``t_YU845IZy1q&hLd(gIqA=^8RX?N{+QH7Zb{J6F zB`yy&wb8ks2*(CQX~CqLxvS<@dQ;LTEvV`pbLyroOQ+;atnZ!KTs6O`wIHveta?G? zocZG-e`%c9IBxv${F@}Bba2L!Bo4B~L{y}%Ck#SUqav+V*$^A$ zm=IND6BCIT%t>DV(@)~b$v^v`@WlV|8Bn$f7Cm7}>WF6zX9ZR3Z-q15I<0kTXGhb- zsxcL1#pulo+!%vS2Mdy&X_y1R(3)#;AegLQG&VoW~d(qL1Vo%bWRRA>}*)w4|wK`-qizNVoG*T4}Yq-{L08Fd|M^jOk{_62mS&_v{bz( zmmL*OaU~(TEZ?I?yZQnPMoIMnGP%&zV}|4)v@`SpOm7OPV4^jC!Znyau;_4;3pd1~M`+GvC1u$=5IW0@Eyps_}C_|fasBU{vS zz!6nqikaMvg2G2ZL5Nc-owSWg-*rSfX&aTkdt|z_7F7D4k?GP}Q0aS*NGEGSrC(0z z&|0Gpm-C3M0T(>%E%)TPg{589gh}GV2a}<^s4{4LI&lE%siDgX3rw=Tp5`_Oh|CYP1l-X|*N{Ku0&w(7MGU(EP~VhP5E=rAD24Ua~4m zrP-=4_#f}3kO@MCxDI_gToV|(*!%ZKUxToJl%*+!DN%-c3UafZjD$lGYjUn!%D1(kEz7*2Ehn?=u#-aZ?UW~4veAI4bJ>FC zXpMaGqp>IUjCc&nj^_xhZ9~}~!vln|o5I7eiB{oH9c)aRE$0pAQ`C`nA`l{|TWIK_ zVp3>6Q-ghR_C#;JMeRwoQSE?d!a_=Xk8&+W~jh^RczrpiqPA#lh zya(D zz}x>u97Hs#Bb)?(5K9q_B_p}y~OA#{lZtXLa?nyjdGj<++9qV*uFY#CD@y4zZ zA4RVb#=hkv`Vsm)@{Y{Ee1_x@B0%asu?FReJ$o-VB6mk;@!cJr#TP4k7Ml`I%c345 z5n+Nv@GB$1Dfhb9O2&( z4_bpOuKju-xD7nX04HuMscGbamdJDCYNj1*w}TB>sm%m}Z5Tfq2q)&b>`9n$LK&ji zUl!G|F^F7hhJl_Bwub8FbI~0(NQ=XQUu8r#_M__LJ**%A2gM$6$WC6);gcf+QG7EQQg!ucc^3<{h8Wi$*wj#o!K>oB6nhUWE0;~TQo7UJW51*W1w)7KS+6p zT*4~Kh1Jq_pABL!$2TeCcF8G{f7M@OzO!4-?`(*NhbTS>H46Q9zawZ5$o|x!vZPFs zsM)l%!|GW8vnjQ(s0?eBOMwD=X8Uo`zheWRdYAJ4mZFP%E{%>qmIf=S@y8 z8C&4;x<++aybCHX?MW>vD(eDwKJ~a#yuSX8nK>@c+ds)E(qxKLXg#w1EJeGbnpEke zc?PJ6h+bF7bT{-xiZcVHkz<-1I-crT4xdFi1hJKoCek8c2=o`twT6o$(+BPg*c{~) zcGD39Eov0iFpOiCajb~^D_>Kcr>NAKT2xF%va0&>+10rd9D#}BcnQ@x8Ourtt{$#? z>X%fBC>BC>KN-=bbd)F=(KF22rA3LVH$sl0HalX0WUGv@Pb}Z-cScTNyC|FS zbgB`RzI$Z4wEI;0o{{NttW2d}enfg}{5e+?jd;F{5~=I4iwOQ{$mdDm&jql@^AKIc zXrvQ9jnAv5Y05}G78*onX;Mp#2K9-LG0m8tpOuv#$oFUEWw}cb1<#80+3~rWP%qGj zaQ zU1n@{ZYz53v{8JU7FqV(X%&&vG{rOIUGn(vD9iVGOtPTLs$@Ys9Dyt>hb(vz5vyW7 z;h6&2G}B-h#w?;^Tq%jkn$@ZgCK*E4sVT>Yt;Cd3qkL%fqpC*X9A#gHue`{_ z<2-VhDiyB4&ka*yiB3%@7S@oHA+yv#mB^YYBLB9As&dQRMc$_3ny$>umfGH)?!NI& z_R__XUze7S@fF%iD&MTI+%L>Zk$c9r_@GqPm@!ospS)vd_1NXDOINR1SUfOw^%5~< zUjHme|58{iB>gn%v?7cl_GDPA@S`vmkN$#1n=j^R(1?50d)R%;)btq1a_mp>+R)-yMrEB;5bIGQi* z{-nqUBeF$$r5xiWxzN@*Y#HnbCwQpoypu-oN3xX>TdDs8-(uM!wo($YeFR%$KZC7l zu!7Ta(z3Cp5#2%1F4^j;4cUrpV6B`}D_2I>)XEW)wYAoco^kUYU$tOu!-SINY_L`2 zw$%^iC-Ic2+`eUR>+{Q(RRp;O!*q0p@ylWaOUw_q3&EpS8v zpNvj133}d(yU|)!z*^){i{nT~JUWdTErhNU>wo&faM{RfVgU?!{SPAm=p!j)fw{&q z;ObK$3zs#3X$@AyDb(U7*?sW$OH*2nEuqzse|2|XlHnbIeonsom^~Z3kN~d1YjvF}ZgcVJ_Wsp`or@_qedDBtLa-K^x z8bop#8&<(RK^7?=y{mwPMDRlT6o6CN4Gn z7z?|meQ%1XH4K*h1V4X-W4mLVjN41ZNs^3S$;pN2X-2_=t7ey|7M7F+{yD)vGB=}N zNK)dX`$FhYu(}Rw6M_U=iZPn1t9)oTOqKZ07=ai$hZ*A(xDOtuE@*V~h~Lm|$TX{F zRbxh1l;vh;+ARs0s&`gHF|GoE3B9DPC2;+Za!<9}M^YPma{-$Wt}j63o_8W}p?5$NVzdm4H8AuTi?c$3Cnm<;pHFjWY4FG4 zPr?X?sb-?cVbOR_WcXC(Mp)gHo3(3Ier~^=a^$unN9Tvv$Skc1pNBj_&Qy-RL(BXK z8HC^48&;lin4DM2G3GXbS0(J!a9$AOz@|+kSWA%K;b{uFIVgf^q$Wc_65@^Yh|(a; zL?aH(323H;>biI{{fP@PE}TiMcVe;wjSK&9#{!yl8s|W?F3|rw`MpQXb!coGQ8n3Z zfsykZ<|KDF98pslnb?4ccmpj=WtY5>ZH??Y@=a*wLb~)mlPxk-uG*ZPq({0SVlIi0 z@;;-9GIWO~I~Q#RstVpGN(MV2r>s^^K&It-u}KQH&;wTRNviDVb%-&b&q=Qv@%-&a}p}BD?J!bE#>nWbA z(qr~MrGKrFy|2<^_P%<~p?JEo_la9>$t^z{45#O%Vl3E$0bvRO#Ri2jD5_~-8*G!g z&^yiK!%5Rb&PvXfJm(9(cM(lXPILquMFF+)4))>}Uv0fT3~D!lB}u;`5PrSQJ~M zOs+ad?cXv`H9@gAa!!oj2ua3yP4nSYnXf{gNJ12k$nIC^%I-(B3l)+F36XP>QB<^Z znmr{&gMwzZ=$u(OWtPuf^w^v|m(3RGkqP_>$QSy0F$s64uum3$`?tBN{>m3x}GFOrAtXcImY3kUX){aW-{6e>^&Kr zp*|k%11}z)nT*ArA=sQ)*HdI2_xP<#w`^T>>*GgVd@;}Z_mfY46?yNSckt9kco?~O zY8pf2lMDnk*}45l24~X(UJMA5$6u?M!WVmvUX)TjF_@lr|G@4``?bNNlUfqZS~x6` zG5mSrh>)(?WH60 zhV*D2Zm&Q(Y8eqK(FuFT%{GLSXlEQd#usC=D1tGoZoCXd>H}8TF%1|$!c7C1t`OC{ ze$1c4Ldrvd4KJl zGbc1q+eK3?j^QQqF;o$$;?K5(D?5s4zD0ASN{;BI!Oj?vQT1P|{=wnS72Im!h<9K^ zPCsV}G#~c+sX9`lR+=BuEoWYg$0SSG^_XiV4HIl#5?@=T47S3bxDqizT7ns~BgjCu zLXTKErkIR);P)j7m0UPt!U^Im(Oy@y7Uzv?H7hrqlyqG0 z;NZHR%1~Es4m^b9a99*4_cvoWhnUpfZ_gMy-|tR_lGK#Uf=4tny_B67&d$Z`m>k$( zCZ>%gp>xIDbQ*S;tcqF0U^J;o>IUYF7)%c3U@%(KV8>$d8M$(D%n;LuNN;KavnRiQ z%kXtrT3V3L7!4W*eEEe1g~*88A1sgV=mY0Oer|MycByTZ9*vfEb;N=mfvP&8#Q{KJI^z@p}9SO zdd~QQcc&B<733}&`WBs@#uh^KzX3PM$IoXARhc~qNx(j3OROj*$EGZt%LuU?Ag0i8 zI3H>F5v2t?)U}BE#(p0NXOVZGpXbgc4?ojR9=;hLt(G-QQa&=~C<}GYnz3^y%v%?l zJ7HXSV%@lj6UY5FvJpE%e!Ov6)3R}!mo+UJ7oIk4y8M5r8!iI7iErTt#0%(IwuY0^ zIY-ol91vBGT!^SPh@MNs-c{($5T{jNq}Sntx9_6wc#H<=q|4XkX2fqWM^_}0d<4S=?uu_2(SaAqYeB(yFEX%G_5@>9M!qvh2@_=J35%> z_>9jgcEP~VSoPsGa*w5K%;+@PjPTS`H^wejyw+h`+U^yX)V_V6o0xz7(Ba9S28)TN3yp zw0ny-lcl6s2QaK`wK}>G)UsIEfE>ulq^$#jML~?A2mA&3ZnR)m7WQP2|1xYAD8~RT zxcR37xM2olNvXz@<~UY<4NK!RA>JvDcHIOFy7u-}&Hv}XJ+G}+zUF(vX&ZE}L& zbME92Z;3p*bLY+%Dkq<}E)w}WFO+zY>yQ27sw%EUhN$yIt)oN%l*mEe8fG-<$x^0B z6flKo9t=jgACiMyRMY}Q!NDBlE?F$to`mvg3s}>HGz1(zn+UX~H{FoqD6Od~%=ZT~ ziuh!(RUGx~l~;8H!r4`)ZNsiydl)}_6YLDyA;QTJ158FlE3hnM!UKqid-$baM8L12?1T91ZphPF;t8$yUL zX}2NjyQv?fT@VGzijQm={uJOltrv$4fysh2y%y`mA;h#T6mk@uFP$l-#8!+4eHm6u zP8s)MuRS?dk33;R85JPquSeeKKl$7Xw%x&ZL|$4lqi)ciNpQwu6csz>L)b=Ba2F7v4z47p}q_gmXDAFC{0>orQ=W%gnaf z#@G_UF)2R2n4qKWQj3++CVV6veMeha`Q4BsKOjRXZF46jx28hFOw}bXY!_< z$+;O%4eb#Hr(APhj~it?31#`oz1vX0s5EEB_ilp>{a^NO!^$}BBZe7A?v=J0mY3Qq zP42mdoTT8QDd3Hh_OMFB#+q<4bgb-!PGG{i2(#EZcXTD73kFN5S&>}RwhQzp8H&I` zD+n>miCRC}esnSZ?^g6|J-U0aVOqo7Sx0Z}Ti!5@w$H-e#cd?g5)Vgq$uwGU7J!YE zkG1n9Y+|^f&=fRSAT%2W{AgRUSPxDMsAy7lE;Vv#%eZBhN?38w7Yvq@bTc{0b9hcY z0!&c}4`8{B9R0v_I85{j@kOiaW;9M(7~V8}^0tbaS*vFZ%$~eCY30P;?&g}l($Q7N zw1`J4W>rtwLaYSs8NPO-u&=(inEKNuij#@@*Wj zNd;EkOv$u4Gj!-6a0qif@E23^YPCRjpVj)^`o6SpH=uo8*LQ3q+QoD}@AMOgu0`9B zPt{IAEf&gJd>VhL^{I5KU6rm@e`0q;NcaL=PbWl74o}K=3M;lxNJ_-MHM9y5Q6=cB zCD_3bbt>i&4;|9UfPl%6zc zkqdda$R2pQh|G1$WlZqM!zHn!T1HPfEN}Pr_09b2{P+K}@^#uE4R3gZ`y(V>lfW_g z2B~Gp)qs|vc|mw{IwX$jEu96zUQ8&1{|LtcvZ$tDpbg6Rx|If_75T%x2lV!XBfE~d zNA??dYXM$GyD16QGF)%+dl}{8r}plFV8f!JO(G!Ja_=4p4)#iot$_I6RvZ7Ty?aK; zsN8jc-mlasF&GLDER_PPv~+aGJAPNcST=M8bx0(4$KfX01x4Ug0{Sjju%f*RTEU9% z#rU08;ln(|^Et}}cU$p2oxwX#mE46XNAsUJJC^kFojtR9I;YGMI}@+iwdX2JLxXYE-d$HD zjz4L`soRfF;9o{k5{}<~>IMyYhE*unXUn;$czcL44i4F=|F%&F2eX)RiQkmrD|@<_ z@Y@;Y(qIu2Wf8J}L}aU$42e-Qs^_fjo7B8;f;1v>{rT(W@ubL5Ys&;a70)CM(Tw*+ zcMH|nENFT3rP%Ui*meas5r&~gCR>>_SEZ!_L$0|kEwYq0LL@usz|d~7lI$be6A$YM z=(*T~D%xAnli3_R3_XT?ScV1tmyh;SjQ%G|Bl@hWH`!+c+1Ckek}RGXz)6A z0f%JBT+c+lRj#FtOAm*&j=#dVvn_3Z|KugJ`*%t{Kw+$e++(JkC_>(8pP&*1wy|xE zj)|4<#K>Sl=4pQgw6^5rWJj{ykKKEyF5|oR-~};D?A{eS&beUgultXk*4y934=i7^ zVKsEx!;x2;dZx7EP56gZS;Mwpe6OHk6PQNT)$3xmAMF-IdNXeM!Dx@a@z1O1jZbX; z^Oh++>XpZ?5=*DX->Jo=Hwpb*mfE2Ri&PL-S)!x}ekMn(Tb2-m-&yHSY*2)ElI=VE z&Jc&zj#`|7jT5(R-M(?}Z2tJ{*^$Q1j>$XufumQfT}R@J^8PCFQu~aZC@K@sC9_f9 zOtnr6(-05`YJM;W*TR>U8Wb$G^rhqo=1jIPbmz=oy<2|jD_*IKntY6CKMXm#LKIV?dZIOSt8tc0zLbt?r9ujgN0M6>cv9Zw3 zd9=qXB-%jDFc?!%GS~`%hDOCANKxFFiq0f<(;=j|$sP2=b5X{WTwNsh)S{^r(RFTW z?G)cPzqF!tX5TSMd`#qzmJM^}HIE7|Z|gs!Yx)`3S<3IL9&_x89j9z>sPmQ1x`sB% zrq)-AywU!%Zt|ycvSI0gXV4chE06Yiif0Vjj&_E~n45upb~Bo>_D#1h8rszBTY^{nZ`rsI1YkajPVU|X6K?r=%$8tEX? zg_zr^%xUCJT`c*GF_kQb10J;B;dAH6m#=nf5eiy0Z|fn)+@s(%_89jyn{R ztMEhU42=H0mn^<^{Dd>M-}~zsQ_tPXo&S%y_W+NpIQPbP&T6%mR(+eaDy!}4lC|n} zwd%dCntPRd$Hq1`I2epW2h(wYRJgf>m=a@*QwW5F1airDLlP1a63R^iAs4s=a+CPz z|9fZ7*^{Kr!0jNvP-5ntg!>vf z2CP|zLE=dCnO#P=)eXRfrz%K+s3>4cenzXg^g?P6hnj@`+MS+fSs}CTPys3%qF&svq$ym2ap_qb}l`w z8tkwieffJao8W87zb%3nvI3qX~03F4pPsVligovZ$iR!2vA|4qB z;XJx?ApgSs_;FIq_E;lelS#Qbsi}x-#yN7vLu(eMdjh41_2U%5Nss&D=#i+4pQdVQ zG^f3M#}&Gc$;r<6tF{z3+6&V2vTruN`Fnk+%UO`^v1a`HJ@R*@Q}WBV+-ow|CMCSn zbM#OO1oB9w5CiUO$d`%48UUbUg($9+>SSOWk!lZ#Bq1RoDItmOrPLITRneg6AfZ@@ z)~9FJ)s56mEkV7IfI_1u{!nBdXG(b$@Nueh+ z5nAUO%7ks~je{7+-)XT}(kJ(^Wlc(gGq@C!<&K(K4b;b`=R_w-N;OV6pLI%vEVO23LIRT@`BrSBd0LZOlZ2u@3s>+msYbIJQvw6k-A0|u%tfJ zK+x#(^gfEM%FV%JV)e16LguR`Pg~(S&MySo&9ItPi=$jwO{taie$C8P-ouxAj!o^o zc$vS_>~ZHd2S&!e)Z4%LKjojXn_tP4-rrg{JY2X{KDKGa(rt0EiD@aY?$=}m3l>)| zO1Z4FIq+V0_q({E0rvBZ&XUy6Z)~~~DI2cDjuOL$7^9I!@b45lZ<@X#; zt|@hTv!nyItn=T**?5$_U0ab>gK_e4X#WekDl}(Aom?qpX}&KW+GLI~sFc{J+~eb{z##w5JypxTyJwyJ3xVo;ysW6LD=$y}rSx$3`>3-H3>JFt8jRPb zX$drCBteB>J>U$uRSkNHI0dQY%i}4ne1vUdoHmV(Dv$^`7R-xy1;?s^FDk4w8kv>W zbS9^>>4t&EXmEB7ROA>KVV+n3p#3$il?%RKgv~Z+1_HgMC6X?-7PeP-dud}VuzN{b zXk;iEhl_<|wHTDJ!R4>oT)#YMKM}jnCO9v3n(qX zWAIYCun_sI{P1@>#AKsHIgCK++VtGecyI6co;@SqUhz}@ZbQeaRUL;2rlt2{e{%^U ziL5gQ$Zn8$wRA|ev8!<=CDcz4axZ@;sGOup5-;XFLE#6=Y+so?@RQD`SN!~~O)tRM z@Fd|FDjwX9cd0m7vZgFhjQ#qtt2rNLA~PNdCxm4YB-Wx-2ce~m)Cb`>Qzp33FsPYB z_iPrNEsRl|CEW@6il3$U@e7p?CG&C`M#b(Pi=ysmu(z$%MVFw&wmbr!CQGz6ftOE zZzJp-R3Uq+!WkobdoDC6XaO^(LI?H;p=|;+1_dN=j?cn_?VP4igDfL~CgdArR#?bN zaJ#`f2Y{@Sa2gnnHipAE9JZQ;g6DhP9&Z6DS#Z#{@@#`X0>lLdp-RpCu0rlL;irS3 z2!y;;Rf9UQW~rmSDbtfwo8ohOazdB3_Z;e~A1x?$ws`ud*G(omJ^4MQePi#~vkQw@ zm&frb2UBiGJk1-2g3eUA4B1t4`c>m49hz`B3RfP)O}o-YeHa{(@93U@)GJ1|62;got@s~}lOlsDX_1-@V8cIlu~$8eQ;t|zpj z63`Tci-|Q>&wkokSDR-sW?Qndz24qE!FkoUTDH}7ls9JhQ)4itvEoFl>Nb*SFH(Q6LmPT0|T0^vq%evv=;{Oe>n=h=g;Sa!-2 zD)GFWk-VWvgo(mS4;(ad+mk4xUi9KP3tR+m0Vo0{BK#uvu01k&WX+E#n<#5;`}^P9 z;PHlpq8PIsv!G1eJRfqBVUaEd(me_yiY-h|bi}AQlf)cxn7N2gz!twHwZd}A`EnEZ zGS7dyj&h-%qT{TjKULD9zJ&Mr@jk-PEt;A@C2YMA(kNXXxUvKh9*#M&fO|B{70pt8 zuSH|w*gbay;4hvF-z%&n0^t+J&SpdS0=rhN23F0MK#B#|Fw_Ye0+e0yjVginjGcyq zQ??ea30!>zib6T~&8kH!kgfJmp%t|!XRjj1kX2(%O*1-7VebWUqCwP)$dqiw<$@3( zKr8i?b#bseTvFEp^AGedTT#N6b;9y+B=-IPPAG=)0hAlXzHozbM&i2UxdD%NEKGN_ zCr|iN!CDzLX^8JO5=0J`7Y&j5Q%Tl3yLqCCM68Jk_hcPOS8uCAKHdE6Okz+pMlk0i1pc!#(g0kM?Qs7azRp@{}5Rcqx3v?DF~ z70&#g<(n_lHMjd};|;)o8rC+i+%cjH)HWc-U@-H@0jsql@Wqp?rL3aTW^tsJuf3`N z)cuyD`wv}4e48OJ1&x0(e0MJkq`F;_PEX^{G{T_F0Tn8{Np}bON;`p)QEoRJyS3zd zN3{qsA*hVx<8GyJ9M3{sc{;CE;cw`np`@0Vm)DfnVD47R;zcOACNg(M=cJNm6 z<$oeEib|vMO-wG?67#Wy@g_$SQ1p}~eg67|$OO%9E@sRn8pt}>9JC)fFC?ukN(&*`D$PV12nd0_7v(@iC}Sn!PB>Bm z7RJsl>2GT(h5BJ_3bg93nV4An;PS`WzHKZ1pm-DgsjDLnzA?D=Uu0$Se_% z`qzA_xH~|URXkN{hy6czceG`^sr?soovOIqe-tz!q}Qye43q;p#0^Ovf^<5ykgkPP zjomY!8g2(colzBbq6M-@Z5>4{J0={d;F2uVYb4Y+43kCGOl;xz93U?*l~>f z>ni6Ne4CTn>mwHi@!k`!{A9M#S6N}yOvMPW_l$-`K#|GAB8IZ5?ZkaWQp$GPq_Ba` zR%GbbN;N~bBoW&~vqrHes`eBq_GFZn^>@08ExPH6ty<%-@4)J12VF#kjm!7t$`1Bw z-H}?g=F|CGaAKhPORaQ5Z~=u+NVuJ>Euvkuws0>gexhI*pc7@L6GgR4{sLPo)w;4J(#Oo=~tt=-`P3 z?FaawmUWY8BT~Uqco@ICRBj*__b+G`bbnK#Kh^JuR~QJl)9i#l zk~%w%#_`#eirGP31h5w6_hJNkfwjYGA{9mqPCNlxn2e1leNxSSCR1Xf$%bqamPBJ> zn%U!61dowf4l%$VX(IrjpF^8y2YyPl5Z)(53*mk8Rj99#cA|x7|H`cPC&TUkRwMdzS&l!S z8t`6vK6b!ya1BxK6O{;f3xh}r!azo}WgiLVA}FU3+P-)nt=?nFqF31GzFPuODEc;F8(uyb6F@-8l-98?h3bviI* z#T19j9S;4mXlfxODJ3qkt2xo zN&mO5yLSKfi633Gb7=gFTc@XYGyVC&PG;}!y#_lGc=Y?+0+|i@6_n3~zzW+5f)8Q= z{!#57x;})3kA&G6k&YQ0C?_W;FURLbu%^d_;*G#7_$Gi!QqkD?D7ZR{WE_5tS|0!h zw|i+%VIZ|ERM1hlYkSE=R+h66-qAM}o6DLzOT4V6>B^%8-l39Z!CjYNuy2+5r#94g z$lop~dO0-K5ukIE0nSO7vYRA+8@36Cj2Lp@yu92Hd<$1YqTo5x_%;snm^@UIG71Z- zGF%_lNBD};Ux<$ada&>&SWlZ*jWmZGgQH#jgF}-`mR@#6PhV$xk)v?24mBbhj$a<^ zdZ4|jJy_SVc4gNR%Y79EOSV>b&k;cxsq6}`Hflt^D&GmT`;s-D%xKhPL-SX;t0 z?X+`~ldxGa2&l@?W`R+JogX7^h$dI2k;4?qS|ZRk;t44tD`_pI-eo;agDcbGv%Kzd zbGpfp&UD#)5&7CphS(23Jbpva-C(J{;&N83t{_IV7$X|Mh%&kDohp~;0hcK542mwz z&o-PH$d-(Kh7E=vql?SMV$C>b(wANJhMfSTq0LU=$PS3-NW}#p z8|0~JT2;}x{89F^xTl9I+ptsW>Xz)UT{*$d_Vvl3Bg?i_Ql;H`yt5LL4flhFYR_;r0(NJJ- z{I2fFsiDnd6-7sS2KP0u>S!Ep*xD|gD=hRo9JN(7HQg<(Ej`_fOUtWPHTErC6xUl< zHCn={B_#R`4XU@HqzdUZLZ(-3htY~nCIZUHrWP&7M{ldBDw(rAZ54rDdSF(d228Gq z2LkO@VZHbWGAxOxYJ!`r2dGmgDWd?92@X+Ma$0s8iugcE68soZEUH_W)yJ;}Y1QPR zeWAOz)L}<^+{`(8rBc-0v{pKNSu)xai66 z62lC8++e$tbnA z-}QmyX%F%YDQZ818fuMAC03@_R5Jvh8#`&RF^QPn1MXrOv<=Up8& zofiQS`Cmm=@)DZwMp5K!BMxO2gmZ3Lr0_)5s0bELfE!r&90e8!*Q@dHAkzhmiV-wF zycY@6XER+xL16kOdwNqtfp2^^i!G>RPmcuTTY>J^B^Y{u?z`l#NgJbCtw8y07(ZDf zT$;f^p953F$I@KcrhAblb`^Q1#A)= zFjKGz@Z&j#iSz<17&87>SU=Y^k!qA5Qj};i+Q2>`VYdp;>HoidjUI-t(W#XyCypMS z?t`zszx9SYE8B+lN!bAmt5m%`W#hD=_k!p`bM6@eN zj9!Ng!BtW03NEOL6j5}7oN3&^thXZVL-?%6RwD*pJ?c{N*yv!Os+HFeeaSvG*lM5X zUAu~HsXx9sjZXWPEw{zrxZTy!k9wlG$<#VelBk!Y=>=y@kp7I(`bg(;!Rr!?5&5GT zB{~7*!lboyX4LB}3r-7JzAUf9j<49ffZedjsy0a`7G!C`MMB#YU@UoS;m9NHq{Yv! zoG7bZ-@5LQbF?oIbh(4eOKV(u+eB4%=dO~$%#J4Zr@C$3qZc>ccH5p4t8CVs^p<>A zYSCzCaiu&kw)vs*s=$g3kcmJ)NV7H|G#Nkz+5#;=PFN}=CaMnt*i2}RaI;mk#;l0e z2xeuorKH%LHbh{iWTd3$Io+f+a&;K=RrGzm7YUq%j7JmjqJXNO{h#1xRz1CZY~7x+ z`nyZFR@XPSw2$=G?Ow^Of%<^~Sn#@ng|*2~#NOd4%6H4#bE_-74`X@n!Dtf@&#xmN zcnDEEblOg!}j z_;&weB+UZfr(9T8++(=Trl~K*aRArn7YGtu(kI&8b3Z^3H<}g-l}o@VLM%f%aRo*q zC0~IjLc86DTH~;lKuU#8D=h&6G^MS;qJ%@{%=1F#5I>PZ1{Th2)sQ({2qH36jh6Ex z2CySZelWSiosnbK8(3)U5xw5zw8yao;6^8YVQJjH@~d(cyK?2a?M)VpW)M5~)79w6 z>LsEPf&gC^YgCO6-0@W;x+{{8Y9T~Xr~m639hBMOt3+uEcdk}a?88Ntfq)3Ya`xw*LZX7wd zYkXNvPtD5FT?fZDR`q=J5wh(596uk(Z+-+{G^VKn{d71-9?dI(WHnU}Qg7>G$t|(~ z+lflD!qC1E%D#p>^pg@m@EHgS?-;7c0l#qMgDOO4ujowB;V1Mi`t*Bh7~L|tn<3>PWf&*&f%5Au;`7T4E!f+>za@Kf$`RkNJR#zXnz?!3J1HB0x^ zEzWJuzhcW(Q+dOiJA008*0IVW_i(zUJjdDFUQnCFa%w!o6WvvH6~*IipyWD`ld}CJ zYpeki7$(+B5hYRgoySi(5N%!sgcMkeDb{TCvkyXXzu6F4^s7xD_Dt*KF)8_@=50Fm z0AWS^X;9&v4EgG@f1aJeAK`Y|SE5~H41r%iPUT|sr}(~ofuz~-ePSUI#3rKqV5t!x zi0Ujxf-eI7G6T3t@!{M^K(iVNI2Ew{bHRYHLn+gxxW?BJ3>eRY0qbHzjm#Kqch<$f zJUy}~_LU9NRZ+o!Prh{G!;2>5W)P%qS2p{$><4`}gABExZ-T}ba0AjHKo`Os^wQN} zJ{mLkdgk%4Fcoug4vF>s;p4D5wCRqOkGwGb&<3RF{n^iO0RHk9(33xv^VUZ>oloE( z(zyV($7H^muD}A(e_>nc{OG@G2a`kJONh@dFL!4Ohm0YPG0(h_@0TJO0D|g`Xj>Dk_x>n?Q_B zqZdIfP9E}Z-?eckV@v$S^5@vv6j$Ytd>)>4cQA^cQg^69V74!46vhpkQE6Pf;^0ItL?%;+iI`RRS6`l#$f|92S{DKnt zTh;n#BHd?sbmmevl3&}?*xR07V(c@PrMGvt^j7(HxLh8S-DO?K{?3vsjbOY_Z8tXQr>)wsR%%K%n8kzY)3# zB$a4BhF^cgzj<68MM~n9j6_PV6FVF|$sG;Is&bkXiy0ghI2wdNPWH!;eY~u15LHEd zBi7KXnYqI~FFspY;>dglZ&?8j3y!3j9Up1_RPm97IZ4fsZ1&(Bk;OBaHjG?FqX%*e zTNOQ5jgKULi!=1`@sU0`e(=hHV*|>g@Hl!F#<&B|N){1#YJ4Qzf&a<)$Q>A~g+D(s z+VF_i&1~a0ok0WMh_{OPNbGLlUE%o1Feh<4K+TmgOB42B3po6U&n_RwxxSN#x)*BTq^%a<>7E;|1-faPji^_HIq=8iz zd3HKKe#VfB?baMbM2c8YCL|@0I1GMOJHm5kTM{?!+`WF~=2%@$WtlxaCp{K&c)+*#>~vBwrvV5_GvhOr$H9$3*fm(U8c( z&hIB84p-3F_QXv)URr(m=;-Cc#sYtKU66J4kAE3K&L2w2-~Yb+_vexd3u}wOL3oxi zeha+GIf!}-WRVvlqb#5JMVtiL#NdR8Jw7`m5|`X8Tooaa2;EUaA{AIQ5~l-K90GhP zcM?0>ljjV2o5$nceNQ*u<_kJ~&Q--1AFbcjFWKs<)6L_ot3*X&Y#6+PJ7abzr$j@W8x=_k540~ba*Jda*Y3Y` z@8Z?&{6JsFi0-c+=?8)XD6qU{G3u-yx$L&woSL$UrNPi>DTW3L9>6M6Wg1EI2){@9 zzZa+iq*h;%G~eR=j9__Gr%KYmp%}kwuv2TW5@|qjJ|#ukJk9zIxk+>FJGI*3eukF}MH1&Ze9@u-^bbg*PMocCd&PPeiyW;SpicE-?|o z3+6;qN)o<8LG^?}FxUm4&xzU!Mo}Fbu*rXguHU)x@bYbIx9zB}t8Aq6{`~W=7Ofi? znbDjpD)SfPB}QHg;ce)as{^Sv*ghdsIvqH1u;Rohfg-5Ch24StaB!icO{{8i@GGJW zVK#h#9&nc+Q`oEzkqj9&j1m`xQf4A%8$d4UQWW(7wMx`WAPtL7W+jr|omH@(o6{(vQO(d76$_{a7n%|NI4fYVAgUAX6U>L(`jlNQdaK(S z<^YU_0-ovKNaIMWTLm$Eiulg8-PsI@y;zb_V(<4X-xV7Og<9gbugLFk7TU^kYMbMp z{l2!Xron8k$gv;YO*?$^k*w?rlWA-?I5mltaAPGZu`(Bq8iXnV=y0K^LDDE_XNRK( zo4XnZ$6sFk^Qlc;UC-l(_B2a5dlv73e+1&4NTO3bqmT@7UbG<2y&5fOGMOwUOH`}@ zMGLAgtX61It%NIn-`D1@8)(`vI(n#Y`g6nm6_qdWAH4rOMv%rwV8I9wlZ0F9_biKdr8{czq7(PZwXDzhucj%G(OctK;JGN{f7fuXP0Vz zrW)cM`Ot+3C6J`o_s~r-?XmUWj7o$mX>7aWhJMi8W-I$4H>+N9jh-h*2J1Jis+@!zd#DA5fsl;_+F2xuPe23Ig|0V+ zFNj_g>?5N1($h7XbXU4FC(D>dWe4%HMKBhc3r$p?p`60hW=HKwTznD+;NHPIdj~fS z^{04OOa)uUml-eKG?0_+^WpynmUiTX{O2Plu1*QGW6ns1d;YKMw-{K$>K!{)uim+1 z6;XHvC|rmYxKQLC$jj{;82_xuJ?<1ln--UcL(ftU9}cZp!FI&6AKcaY_S>z$qby+4 zxZxn@YH}&2JR=n=hs%n)hLWc&B$8Mu9I_{tF)R!rWDh<8CYZnHxQ8my_&x{$WT506 z(`;t%4YtAu$(myg7UvZO?JUHC@o!`gmj}mSVXi`)%=z-NqCEBkDY^AYSAM8Vrn#p> z|C+-1GDwb0PyDzzy$GWOAu=>L!$5V_IIVD;o_dU<;a#gV)tKa3SJhLyc68s?u}R>& zOGma&K~eh%KPtfcGa647BJ&8{tzlDvT%5*5sGXpPvjBxbL@Fo50=Md%GKSPIlrlCxvyDP~}JP6GQSpx8g_oAA*1bYCUdxI%mtwS@Uw zKlOJBME;jv`>(wb05byhD=R!M2Q(0rg`jY{3zvmZab@u*SjQ~zS<-EpnjM~aU|fZo zSIz^X$HwByVoGyM{fUOMlzdy-cuc!Mp66lGv`(9`4|}l>KOrtLjGJK#E)bVUn~!uX zGK5RL9Z7CrryNDS8++|9s9psa^=I@poOt57JSq@T6hLNy$AK%swz(R23Bn=uz!{05 zQEZ?|M-hq2UKbIGT#<_cYb){Q&11zSSvO98>D$Zh&nhVfIR4W;dw%*q@*5ua3E##fJ8mI=^@D?hph^y;=Md{UWwu7Q{>_*_e`Lv zN{go-8VwB&hHkxe?2eTWW1iJ*8{>Dk$v>m|gybX9hq)$^Mvu5w5%DPmd~&%ZU%Tj+ z;eA|Q7B@0U#(bJm-}Kj4gbx2?GVVL zt%yZYRt%`ERMnn5{QFSo_k=eID-9sYvNC4H{ihn zS;d19PY?eAKf(CXa}&95Xv|#9L>mYVC+gh62Ov@ zp?e6vqE1t;wbhZESzsQ&dxCWp1=;exCi%VY0Z+975sk^7JfrD<2&I*hUn#(jtVP@* ziU(>uK;OWrjZA{X8YAVw`yL5hDDF@sqni_V2(A~gffvAo(UY&n9l}#YI57(Ip^z0Q z0EdSh5@^9A4#j0-mb^{Xd4;v*1u@x$+YVPvx3^b1y(KMOH(9fC2HS!IFJ@$Ty}8-B zj{7#*%PI;R%XN!xxD26*IgO@`(?2Tg^EOWm1$`|ZQ%|X?5$nT{zA#3vq@h{p03whGZC1U2-84@JOPF=p$llfM)2~=kp?QhCLhi z)bH${?#XX&?Hn8HYH!Q4b$RWLZZ*mmU!gI!IVKc5%!SGeisSZ@clSd;c z0@n~JW)B}JX?`f8=z(oOb}fH0e&G!T^d~o;agzV+P#r-2q#WZ zWOFaxTZZ>q1;?Bdow!c*FeOJ1ke|;FClY~vgqlZ3ya_xj3OP@e%D@w%!E^A2X#AXW z2obRKAU?^vj$u%tCR6v`SZXTKirk)LLzwdJM7NWAk} zoP3|J5bs+B$_Ma1D|xWueZUuc2$bofnPYFkPX&%_h^kiXSc2>xqIx(OHIh)GnqExq zeo_Eb55L{tMXa7)UREqWO)tOKS2W&38F&mpFABjOT<{x3ydjb!d_ue-IGtkUN4y~t zedpspXCXlibK(ubE=0f~M;?en_{HFSoK@qkNVq!GI23HC$Mp74w-xoi=qAJGtH}@=o#_nWa!?u!yCjs{47U)uMgBxU%exD7CVI&6%b|OXD@g#-w)hLoU zko1JqD4d8{H->PNQiVo#vTv{z2~W(qR=aa?!^Tjk?YgD6GS5iw_YgY)9m?ZQ6?$jy<(PNACLy=v_OO8=5s}DWF^9i{us@8TH(n5 zio8ejB{^Anom7U|HvKdd`e_K0R0KZqn;3(&j;y1c>s#i=7_P&~g7hO|45L^sxc-G= z3?qD>{jnpty(4ELY%?W}d*T!h+6y_DCslY5Dzr$Y)x1BkGa!%v2i^Leh zC`B>x;&N559RxL`p*rX6@IrDY(MEYc7^Sgbn{%;A$EHH-nKpUYyM4v@4kxWL>uB+r zk>Yu)wY0rTb%MZh<3w=pM>-M0`%(8SJfXtv+=g+SpOicPD%!b^89zb7m+M(%NFa_x z*is_6DrJoi1RB$zQm2AzaxjT%k3jd41mqv(eF&3;*$iPM%bb~R#aE3^QfP#e$| zGk67$In?q@3bVou`Pt;ylt_^T!x(H&BHYkT(j~ty!X^D&3L2>Ov*U*wx3>3e^!eSr zRUHF^!xO(m6!J~KGrTapx@B!uaY?buKG5CK)cp6ZAY7p^58=)72F5Eqp-$3p!|gN= z(XP%LIZ4285GS#rUl%|y48>ml{zXM?ocgCMkBek7&D9+R&zpH zLMkd_t7)=?3n{nt!Is414v~t8Ol@KJmge>tmZWQKx+*r<)YKXqRiimHIx_b5(y{R+ zG>h{r`u)0AEv_Ve(*&Bixj?M8zCp)m2Du!I;DMNl=GNEBk?bn4BLv`3Nvf8{daN z5O1Ov;Z4h-M-qOBgYqe34)OX=8FOf!6LS~^HV%dHzM(Me_?64WJ=NQ$;EyF3n-^pI zz@gQ*AK3q= zKmEP_zZ=$RmmX=qv^@|pB}lhA zB>U3t#PTSnL@wEfqX8WbDpc(?QLaGl;x1gnM1E4m57(M%jf86<9x#rjR4sm#-U_kCkMYeVK)e3lJ7qfBa}^S$V;ULqnH!W+BI7{P}N{ zS5I!K>5!kwD`Bzr+)eAJHxMNah%yU+k}lGYK0Z2-Z@4f!`1H|%JkBeyVKfl(m2F;I z(+~(=a%j`e`ug(pa>rUkeAU!nbwzXgWJ|GsadB{Dpt{qvv>^BC8q#X}aQ?{aTDipm zlU^_~kn>;!3}KaUi#ujV2F?py1hfa2>(a~by{)F9CHT;(O*{9lhZ^xhP#MG)9~daK zGEk=o$wd_vIoaM4PM%{|hK5-A(S3&l*)_(ZtsDR2A2CNg zrAsz}L>OZ+#zR;61Cm$YtQ zYdz6=zT&#$!-s;Pj~4VnopZ=x3&ksfI8jRm&GQ2BigcZElDO(uRQ9ZTfISfZh0;8K zTg*5stQy~2vtopKJ3Hl9t81pFO9p6NMjScVLDN{h-wh}U>49;^q|yGV!SxA?*Ntpmwteb`tlZFq zB)_X|^n1!(MN`F9x@Y8d)m4^ETgPsH)7~}Dzf!Qacd6mC2d$PRlg13Tdu%+lv|*il zBS|scGeyw#$>008Tn>g84X>dq@tU+u15p5(t;m*zoX~)FDST9q@X<5^C@GN}!NIEq zM5!X!Z0%fIc?+C*4zl6)VTxlSSNi)>&6sx8!I|Rb3-a`OHU(^@QhK z_9A-JZiaO`=VPKrL29@uL<^X>KJqlaOm02l;?%5^v}#6LvYtdxl6f|itOC>c_5`|r zU1om&8l$Z#3GrcT$`7KhVHj0F7}WygrU+6eB1p}knTjf1AcAyZE09}*`v*T{2U;+K zlw?4jZ%)0c=gkAQps~-5kAH3~diW7zkN`+%+>!>ugJi^-r$Mg2V6J*{)|!*6-e#m= zY$_;8KMUom*TdzEf(Lo*;Fq@_+Wpz_@z7@s`v%Ip5BCMw;8@?-6uY*qxw)-T{!2;y zs^>784d8jH*r6uemq!EXPQb)ESB6C!8-q9zEu7BC%&AelMch&0q?$dPiILuc_&f3( zAJgk*w6M50d7jXIz&Kd}T&M7kokAIhS5%^t7WWpWy67g1JC3Osd2*c^;mJo*$f z;|l?t#xJE76!Z>OXWJ(#LY?}Yq0;qBo=aOf*qfSC-MCHDN4XsH~>BHzhd{ zLHv=rnjViQ&y(l#x?LBjt4U0TeCpNc$%yqFk$?!fMfF*iSz0~1B)zsZ@r9(IH6<-o zpY&YWK*m7d68Tqty*|O3e(2J&4?Z|?;>0hLz0>P|@DhGL`1jq@%PY-a15At{I?RJC z(f=A2%RT<1xD2e(KmZ;w5EQEfMU9ZHB04ULTc>AEbN)=y4s1k;kew5Ns*#$3F$v2M z!Gz#+;CzaMs;H((HX{PmUnH(#Y}9bD_|4Z;urWN9Dp*!vyvbB&hD=GkdY2ad2$bmn zNq8UveSCE+k&c8%s#J6)%U?Lcmc;FUaSAzNBsgfEf0dW>Qr<-P%tCLXK$Cy!O@%mX z_aA1X@%x?~rw#FZYpZ;aN5;&|%)re*eKWtiXV=<^)U?oR z;)x1Kgbhy=IAq|qMx5XpFhdV_1MzD5z{xd>;upmu&PxwU>-E;cYT=sd^J%r8uO3G= zizU_8UAs@6qRe@xy4Vi+CXydkVN6+kOeuT}K#GCCV2msqyCX6NikU?`52Q4Vfxj7C zS3@B^csqPTw0NV#fwYVct<-w{C%@bN8XJB9uewdj0gW%kqb^we(?e(*m;Alq>Yw^g zTm7>OPfp?EzYJp$c~>a|;k=kY5044-^6GyS6NoqP`hOy`wF}1be@sxZw~5H+MkPS- zaGos<8EO)sXJ+vz8~Ec;`euT|d2CH_*o~q@)0dHvk(H4JzdVmaug{pBx-AU#K;zWG z>=2c_ZE645SpVSYSik(y3*o$N*Ij?Zb=O{Z-E~=Am?oFyc+Dmrr$qioJnM#s2McnP z!plqasIwzW3a{v1>0s~Od8fSZ&O6!F(w6hTY;TvmR4H;3Xh)fmBuzc1T{`|v*@a^@ znSpc&gOpE7k~}=@msUAVtTNB%<(Hb4oPBcQ*W0C4=kMdyZE;OqpaLX#59rvB&kcCG z7JF30oFUQ!26N&3dyksja7t1Gl*R39yV4F8nu8Yl^dywU#Z4^5I z<&qHXNtN*6t(9Pa>oUdFQ`(_!x;+S~!kZL&(q1)x}ZC7%GxDtmdz?RdOB9 zK4Z_y_x!%QyBjsUl6rgo+(i5!4{tzXW6WqRJqO~3MxxB2i!|8*8x`5YJm6kQtOuWC zS3p1JXWx}7yinNW^&b&N@BpQ)mT^I7vEpiuAgWXeyBP)@hjKY6p0pOaDU?=J zK_y>TJXZoHjpy2PY#CIk5)m12bWJhnQ9?Mt$G^-h?H}}gTy_2j`6suJ!g2;Y3+!A>4;+B@C-TnQ$n}RL-|D(BO zWLVmB#myIe*51_s6DK^X2pQ9;N+V-hK?v zY2ob$mG*mh`_IwN`%3`e_;dc7+DW@*?`e^Na!%DczH50qJqMX4X&RvPZv6T$tg2q) z!WpF~bvOpB7T1|dUU7r!aG!~qyAJbnxD>M0YoU{#9Qpb|%abbmZko6Wg{b9^zKOE4>%YwmG@}S#@zTh_)vEd4AQZUw zG*w&1ffsqtkuF5Ne+V1lIamrb(2k5WFPu55X&^2r31MRiDJdzLDK?+aNOL1EU!~+L z2P>jo=B50t(_>fU8E^}LQvaKaI9$a$MRL`;9oy(eS?v9$Oevn_t6qIE;S%VrHaGOW$ zD~>1aCx#P*^geLtCOA=LDENSTn0g#=880*1AtFX3MWe!Dk&Vqr*o+kd>9KeO-Pa^>=GuW0R?@ zsuu~UU&I}t$-t8+XQM&m4Y-iMpOLGWxMmUFhGi3H9J>a_HCiZ#R_inRkR8G2XY9?5 zOG3vB&Mx`0H^kyuw*0!pF5f99NTKB{=dCKvY8h zP>sfem~7lgVWp#hI4)&0P0@qTBDsYt8gZ?3G0e*7Ux$dB@1v|dXU+uD>IN3{cqN-z?-P&rV+{F zuyp*^6s4;0FYIb6RRw{Ix1&^5nkkofu!isxEshp0Aq3{|VpU~HrtIV!Cth0hrOnxO zi|rXEX>W#IenI|_-%MZrAXsJyfOp-%M+x2~t)}M#e7m3X7Cw67A-rATAvmF`oFO6V zhED7je7^`QDrqM0eabOQ5uoR=(!4^Z29_n5-BE~(kDg8{A3irAmXI$bdZxoAjTT!& zTM|Rf=eP^4o(qa`PQB&#ZB)o-usfDjhR$+B4i4E_=2@PciNb&~w(X z7o@ErJnDHgjv6%9KYV{n)!33+-D5MIGbc514@>OK%tg4nODYOyayg zBp`v9w;=S1Y2r)O}bMAJl(d!CtrKZ4q> zMg4W61LTP>z-1@Wj?CCfyP753G}DIXKdiKCb*H4;MStE-yoR3R66=EX;h)c4pE-T` z`Yhac=0@IEStmS~uTym2==I`#mGwejzFrsTd;gs0@^!mF->>n$%KG8CeElxa_ZaW1 ztRwpJb)3^Tyq>(TvYzP6*Yg5>e~-oC@~H%k7JVL*|HAh)i|qRu1d`3_b6S35UY~Z} zhv+Gu^SJzFz86^(9Xoj+qNC_@W`RDTInQ}Qer(=zmhwJCH}Raa@{{xW{DSwP{hCP} zQunmX<==Vo_%n(;eh&S)JT8$jhUoCad2@VlPM_2A*?E2NHav&uAfEHMe964A0VhHq zqJ!vjW`RCrmWY1N6Y|ybozMQccW7*FImPssPo>(j>j5Z%Oc&dOh! z*9Z4Ao!B-* z$Fb&f<%rBVeawEU}ibG!$AICepjKJWA3ynRS=SqtZtqR$z5_uRc9&+tC9e?*_N z^2&LAXgn=^Z-_qUH};&ww%!OeSh?MDx5S=JYu&e}5iLAloZzI-nuCJuV-dJKveCIepG7(5G@v zpC{y7=RK!kPM@>#$$5Qp=k$3}zI`5T?Q{B^lkb?<=YHOY_6+95?`*Nho}SbG404qd zolC(N#oiYEPf2(2^?HT3tIwf324@g-;-AmCW1g7Pm(%+KeQmSeM|TY99^E&J?`6&D z%jtiCzLj(O@^!dC--bDT`FdQSZ|X6nG;q#N^E3cm}N^e5bDrB#=yem)tkiiRrwK~BA)q_* z1%dSl<#6M>1iCVwysXz8I^LripztEtZuPGA&RM5Df7oUWu>> zt5GU`*_zO1GrRWJja973c|g+3@zr(SRs*ue)!fQ5t8*IOycW?Pk_NusQYL)6oxs}R z2SsRYX_gk4so-fqKqSc*y>c2&z7M?}#ehKgf?=fuE@QFsM7y+Tv}%fjq@+MLAcxn- zb`7`-avR)5errj2=R{HMmaeYNwUeC{#hXUgH1@T%_hkp0q<3<1$}>zJhc!FhoZY^@ zr)_UX+eIA{hq7!<`7Pb@6Vn6ZYgSD)^!ULbqkT@1e?1e}1Noh%VJ*4eDI9Ti$aDo1 zg%*=zWY~yIasK;ioV~D7OiF{XZ_aWCk15OXO{rL#DPCNFhbb0rLx z@x(d`?FoJ2gf2nl_NuCCpi{1Dyk`GZUG|p7rhun^$XQ-)DlG4vEbct9KW!raws^Kq z`s&8xhng4Hc}?c*Qg6+;yD_88WwfTH+Nwe=ZC6}3rPq;=6ZWV}CtNudJH>3E{nCSQ zs4nukgan|#lbpn`vY`QRnb8+RTg9A;1Hv~8We%&L(%w{>my#nV`0@(+tfAzDL4C56 zyQ)mGojiXgUNY4;)l zJrD=5I|_RemT0VoZOmV;Zn@OvxPMF(lv<3PuC6im{VsV=XIi5nWvG+YgT|T(ya(y} zfR%HbYCQQpVE-VgY8N&;*%|OAj+h&B$j-p1W|82jEQW_R!vR0}w_ME&1$Fq#d ze-CAhus=S3(wU=gkM+2I&NLI3%1^S;@)ej@8E%47>A%5qU_YjeMO~@M35FOEZJ)M7>~V=qqM&vO0t<_}J?7ft8?Mo(Fbvz7-g=cgKzI7F1VOjCrl(e`9#Nf2GJjhVC( z4;Gj&4jumP*OWUH^qFWEKKQfP-P|LmqS7yo<~xRcE$z)U2Mreo2fclLfp!OupMP+KNU`y2*tzIOb_g_q)^J|D0K~th2g5Ki1q- z>F>|S+DD?;tYl*&2UP|(%@__Hu3_*8s66xXF{@jB&W%c!R5)O*y{)yqy|uM1DpGBF zX_|J{G;Dm7 zW!kOr1e~A;PNBt&(@wllWLqxdd&#YFd9?i9%2uLlaW2@=ra%sgIxkLQJoJEi_0^4JkPtjSKtL+y0%_&cOKMwl?;IxQs1>gZqQGTzcpxom{wn(+!QzsS&L!uUL}|B4D3)-2!}T1ZFtswY;-r$*IfdImTp3QfBC~a3U#94hSwx6WLh7}WOo=NV z*~<9#{QcnDgFCk5WQ4La>-*Y=9d`X_s|(`bvE%ZL`P+sc#vk52dQEWr(&|-fke_Mm z&eo#Vx{u=4Z`i(^c5Bs42{fq;>&@f9UwnvTgWlSgG{0b1QYVIm^NPqIE%2cx+be_EHQECMMK-ShZ=i_n)PKv zfjzf%9_hL0So|~bKVGzbm<`BhTUr(|g2KX?MBN&Aab~1bJ{2mfAxX+sUj~Lxd=Yub zjcynDxP|KxVy8iLR+Ws(%4%U_1i5iZHVyPvH0uq`Ww)32+ESbay+e-tLZ{Kz&yL-W zWWKdE4S)OmWrIhCCsuszQp44wQ!6I=E+;!9mjtwV8mIK1SbRzMFxhU^c8Y~n+V6Nb zs$F|?xcyP3o$q7*oL9wjU^ArWi2Y5^5$$)NIo!^lBibKT+WDeJwP*uimhZTFy(G59U5uSCU4u)#bo?s>)ZkT}B}M5=i$^x52OTe2WO9B1uO?)Qq+Xy3ZUO$H4|OOAxRz(*?kli8MC zTb2~_JCyl&bY*oS-o??F^IV!UjSTah1zq6L#6O_3mi2syly z&j1&>Du|Tus;Wrr>~V3EGZJJ%YMzs3PKdDoIgC91RleS_+X|a<^NWTndbiaz?(8vS zW>=Y0*G&I7&0J;AXuEVQbTEJ=P;*UFZpyAedlH6_3>}zqEaDbJS}bmOMojNo_0A}R z4vdRUkuM^dC&Ah+E{~fGxNcQ^r3BbY7EL-#v+TEbwBIdbLt^28ckbnq&*ldCuhK%JGfD#a$ym~S;wd&$++xFfAfsF*WJ8w?@Zv68}H|{!o z+lkA5OfnnEZrE|{GX?B@sRdc|ECHj*2n~~t3OroV;^QJarFC9Vi}Z0dvKoAvyTIdLs>PX@m|e5Dyw+z)%Pr5V=y%mwigGQP zDG7R0`I63wX>*Fv>%n*~oqpfO>w=Y-Mqsw(o$Un5M@;YL>@pptF+rHaB zk)KqtwDsP}j;H!2@z-U+TPC?qY=>VFgJxtV{$@6%B`3zmYPb?cREC;j%B0IwlLkJ4 zH{!yB^SKvZOAIHORsS$Owcl=-?!Vn=|J8(MX5@H|bfx^Gcly8D9DmK{yW8Jm%c~d5 z^-|o5J^-|op#xVvxQB;sIf@NsJswA{JtN&@OiMsge!9i^R>CdrcJf<1lIE?F3l`PY zTA#{kO?thNV(zVyZJ@1bvD0X`25gp;m|v_j*|O_O#b4@s20T>=r||(2QAC2p-cflV=|1`qb1LO1| z@-l*{IS>)30jlIE@_8(|7CRn-0_|}`x+o2u;58y-l^>Pp^jJBPi3HRJK@kOx{p?%4 z73hlGXKAaewJp zQ9*k*Z32SSmuJB?;zDha|-yawsjs-X{=W8k$L zYmOlj`%aTAC-q)0WbW3ji5% zS_rWMDT_GmXw^Y3HD5%vIK7pAza!S`LwOx8vPDEUX|TDdziV-m-q2Li)t#S{on$UT zO=k9`4Tfa-*`&B!U;D7#mzT?aJiY(oD@ckva>>Q(SzOa1XtS?vsADzL0Z4S44f{x< z6DRL4{N!bTgQO4#QKf+S=ZXC(>hvhbXpJ+WIi;&@q4Se1-vAn|n91k5Z5A-;;Xp#J z6>d|IKnPXqJ!J(TNEQ9W?aTFiuEs%rf=?^HjcbJSO>vEA$)`0tJ3Bu+ugFtOtP^Xg zha^w>H;1S}O_0+Es^RR_^Bxt}Fk|fwd(!)Hd0rKeFY82{tBnAsGJ{Q%YD8~uO$*9VQlT(U4nnI^0 zAr4NVxR+8Gpdp-h;6*64MLHaY4i9`3<>SEvye^l~rS+oJ8v+Zp`q5o$w(Tk$HR&%J z-Wux}M&|58>k6pe(-p{Vw(IuA|=tR&tEdn2>X!q7Yz)A^pQF>SELDm$86AG~w z%ze2Ub&VCL`T)c)OLRb{9Cm~|q5>~66Q`!dL0yjVAp}lIN*T4nQ02L($axFXSKfDD zXn1(*#+{o1aa?N(c!F)s!L~X>!|t~B_`x`03%8$p zC%PR~^pyU0{V}>d^4=4(+GE1~zdNhl5N^LEx?OrB+*UnYI?ew)@_kyb`Jb!n z=G4);&Hr3oKc|k?Z~o`%Iy!ZK?pbUV>Ow4YJ-nRrg5{}WZw{b}B!|5{u{voXuhKTu9@ie$a_!BYNz!S{l6RC&d(3EV|^JG0P8z@zBoVVuY9`T=T9y8 zxk_K0ANxYzk9Q0I*9f& zKaXyY^nU_Uf8;qdZ_)p(vJb@bX}+TU$yx0*Ptkr3S!5#5q5UV??|L`7J@VcYv)YLs zqW^biwG+P=?YBg?bAB({Pf|O4qQyA(L0TX=w@`Ccz(`lPi|O=k8So0CnEOcqm2a!gX50hlt=6JR8yXL6Xp2Tz!L5x1VIU+22EQ2 zW)wBFZHFD#gBpSg^k=T+!qhl`2o0WvUh`PML`m})lt`_nmc<$BSUl=SiAbPI2&0Ih zf?g>l5E!Rjo1{-f8P6D06pM-R^(HfeVbK%{9bmV_#labOL>9(@4C6$tHW+gHXBmX6Dy6CPIC@qS3`1oQkAjVaYor}XB2YFXQIixC@Ikp zZ%McLN|F;3;>_up`T3u-TWFJ7b8`4*wb;KDy#@I_fvOc+;$tu4d`h~>*VAA z61ZuoKXHUlsjx@dxHRw_TmDXs7pz{_1>1JIzKh z>U>2z@omxmq(TSLPJCOmpA$Zl$~cKWqFtrW-1n;V5$(jcMgQ+&x{>)3-xlp~%ZRjd zzAf5MQakvzc#g`q%QU+JaH0S@70s{hv-vfUaUBj{Y%GT@N~oA#w#ur3$*rCDIf`i$ zk1i`M%=dbnxltTC{u4Mfp@xKeAc{I-pJamzu<1*pZI%~c)A!G_Wd6T0{|oWth3Eg+ z0*raq{4c5*eUl~GmTXJb=0#p*S>7$j zyPd^x7RN~l*&quc0Rn`E1QM12Ed)p)(6ZA|b{YyH0g}=MN=plrmQuF14@%P(3Z#^l z7Vy>gJ9F=q_x%%VyLax)nKNh3oO9;PIT}x{nZjEq@?>N~O$J4zq$Di? zW3Ea9MO00BL}$mzV8>G)7z?{qKHS^W+0ok4*f0$j&ivov!nE2=C8331niZcLPgwGo zVa0!#LT2DY>OLd!Cv~4u=f{(;>-UH8z>UEtzsn>}=sn>P>q+Tcfq+ZwglX^V{r_P_$>%^bb@;ZMa-(t$iqh5z(&arbV z@Pq5ZE*Wg{dJMI1NPkUd-FZ3G6bkE1`6P~}Qe!}msj-XXp;$e9ARQ z??bRvr{g>ZdDaz^Zy}6s*j~h*Vk|~@XVjHs2Kg3JjPTsz+@gZKG*em*QoM>7;YpgG zktS;=W@jNEUzRIg7H8ksym{kBysp4MyVq;Cdpvf2Cbu*f4J4>3&PU+@H>PAE1 z^R7OpX=|Rdv#MvS;NeMriw0i)%-K~k?J|stz)SPi)OYgZ>XxQa=L~(FYan<3AGhD z1H%M291;;}=?MJG;AsW_OBn9q=|K6(FeEs*A*}$=I`R;(g~AXe6{uNDok5dTMlv#U zfM<=(Ro;HC_t1}w17mmJeRn(ib&)dbhd-niunUEq?}rgH7!--~)r?ZuBw&0fLnp;# zhLVV|4y%BM#4$)FP2{mHHc@AAL??2);1OX=LX;S%mi17z|A%7cVI%T=u{V_Cg-woy zYns;gZeCE|v7l#WcazQU?q;X+(_hO@KkL-)wM`>|@ccQmds5OFrV$r=el%b5;Q`kZ zo&k$Fqiotv3#JTZ4ZyZMjtoGVQ60}ZvB{%t`eD?>>2-GjodO3zxjf#a7(uD?1_~?u zhJk;*2ZJaa49V7MSU0Z8r*?9K=`(Dk8zQIk+ z58Zjzy-Uj)3$NQz<5#}2WbO^Y%5ydlMpr~WVgCX~6Q%sHMcql&DZGiocJU-jMTjA| zl9}De(v+j@M&5B!GMG~`4vT(NWvB8VF&8ewrkMOa(_js zAqlY<2zHGmp(v?d<)o9-ZX5YRtJB9U!HHIPn`fW4w)D-KJ>MwVEf6n!DTJV zW3J|wKxJo3ODD_l_w}AO*xNgsy*cPCT-MgOxGu0Lw7JtZ(9|;G^Ui7ZSK3TPP5%0} z){ydM$dO;SzNM?T5o0$a@*d~>No=(`$uBl9MfeC}3qnEyEE)V2+=j-DoJ%|TOxmEe zI4ur)0d3Cu6Lk)!?!^fqgS0;yr%(fp8Ewk3J!cHvbVcsoeXL+!L(^DY%PAeM!9qS` zu&cYHg+F=a1=l{X${Y4wzQt41u&S&7)Sz-{jsLt=Bf~Qhw~06ccjM$F*;IVw^NEau z;KP6pp^-Gc3=nlLK(-L%O7+=c2~nkNJelE57Ldjv9x%q|{}yAS+eFSiqrLr%!9nrf zyL4$Uy$alJyUp#k@wV_`!@B#1vVeik|Y>FgOiV3-KPiFh|bgeb}u%o!};>iPHGcbn&h!K)eTVS87!?oi-6 zh?wwr`wV!UH!<$x&7U%HuYE5wzw-Q>+1=Ij>wfbb=d|!i@Z|E$)s{;99bal42Fz2P~>@| zvuQGMjcObk2NnR8%AnoTG3XQVXE5k_OR5GMd**^A?HH&p?(_G}y0$*W(>xe%E}!A@ zS1fDdZB;{+O>@(d7R+j18eCNCatwwq=;{bnH`pD>YbPMz$LaiShJ+X&ar_w&GsA)@ zT-v~9O=QxcI3``PdJa=w1ebo**tfcD@7`ASYN0Z8_%LB;LF7uT%>l>_$Pq(a8pxTz zr4vyq5vzgDt+630k7}G+=5s{;l<63C%;`(8LSof#D<2j#ILFpEuj}bpURLkusqUWD z-(vR{b+e7^_)jy^&RErXYEyYxowImk=3sYf8k_b1Ft=VB`5Iz52f(dM!sG=A{TwSR zx$SGGV#Wynv$9RZwi(=3!MNGS|C;rYas=%AZ{s)S^MT&~ulaUMYm{#{Hh^!}pL_zo z{rX^W9N)GNG`7qw_04Je0=|7Y_;wq1fJ?|HB|kOBx5296p29eSJP`vQ#fNYjfOC#G zZ&zY%A{WWJU9qH+v34F(-Y09{Re3e#9R;E}d6IgU*iTM>`sf?!F z$)kijLXS*l-@&FcLFY4(Ci1PAiQOPULE6TEFS;T^(LNs+j5ny2v7jT_( zX8b}@cC^UbyR`Nx#QkdPy<`lFNZ+Mn+9J}#E=*Ix8#nK$SI);tFXfnYm=&-O0BhnS zST*3S#;mx?t%c+7en3v#LVL-Q4)Z=pFfRGg$N<=@EZ@i~@d?c)@)<-kifK#tz`XH! zBXf%~_e-~FWj>C4%2#P+=s7B57iAuZmiaZx?Bs`V?_fm4FF+aS3TW#A>8MtZg0{xA zdO}(~rJ~G(l4gC2jHApFQO{$M^;(%atqk_X1$|R!9h$wS7a++3#V3*}@OS{!F5yJCotsWiEd7{k2l4eDSe1tMHMZAM4k{l4f;?=w(XeC-zJ&!KG^>pRymp(=rvjW8u(&6l_MB zz&HiX187~2LjH}k>25)>B!8o+4~!w+81k)?NaIe_rYSZAqGzV+g6v0g2L?vw4i3zn zS?cwcm3e0`UcS7qf7!DB#=2mzzHtL;eHKjoTGknjvWi_0$WFMZ>76Qj_Xt_Z#RiwV zu5If3u)cPIf+1ssOLi1}vywaxrBT9xUZKNI6_MP&Y?BHr;z7prRw#sGgN!}m=$r9K zvhv=t0cWv2R2nFnS>E1vw5HnG7Hq2VTCDjsz7lt{&{_8aqfet%m$W-)xJA(fQi#9Fzk0z=VI)eQ^?1T4)H zh!kK_qlET*Mz6!?FB|Evm{VBg4|~0Cca_l?gbp`v4J-)oO)k z;KL+#2;$K+?)cqeDB|$Bje3-^HI-FWJip#m*E_#$=DgA2j#}61%KTKXyTt3Xv$UEz zv$?)D7+yBmx6EF%$YgPNiu1_>pGmhz{>cuDc@Ak#)x9B=PnG6WB^-&%oM!ojo1bHO z`z;@~)+&Djf0cFvch6zo5$>v_72#x0emc&6gAD&`*;;vdxmU1?=v0awZ3lMZQ+?wk zK~$?`kN5g377h%~EAtf>&uI(KddXc~nC~iax^Ap*EG#N7^;I1*ru&L2Lk)hrFFCoS z*i(Zu*QTW8oXiYUPR0efDM_|0lO-<)bh0<{8G9Ce&B0kd$Sb+9uR!`mtb8Q77HAXv zn{+`)e7c(`BtDbLWH;F;<2TGIZgCo1cpPg^)IK_*FVyq^%U+W0s45QjFA5KiY`xfY zOBpLKb(QyZlvQMB)z=2v2Et1gXEdH`_4w?0)nq%pmTh1^;*T>~z86D!7NTuP{4hI< zBFz!UmYzkl%WJV(=@GM4UJmLeme7Rw%c`h8ftT}>gLTBr!OQXVQLKknX?Zv`j+d)r z3zZmU*h$d*#gS9kCbVfV`A(C5`fgHjTCcP<0M@cEy9RXhaz7S<*7~Meu?Tow?iSh_ z+RN6nrva^7Iv6&UVCFifTLTJF5`QPh8!)vFbVoZdr4TH66ABe$HJMTf#-_MJVH@U$ zYAc=Gq5+d#T`JrXh|%qteyRAkYlXG3FuGVY)?l&VhvF9tZ*FcrR*Q117Uk+{f$@9> z@|MuR<5mk0u_P~;vSuJ8dThyHCtfnx8CWz5zDtV+)(nioQBYigbXqvWJk6Tn)YpvY zTA_{yd@IQNrYQl8QmiXtwK!oRWCo*Ez@@GNh*qwy0-9}d(lRg>swxk;-1<6D;tqDQ zyIMj%Pm#SoIqBh+a8{@ev*|jOf-svP>0+r;XVV$c*>pQw%MOd#gebdl9212}Hi}v- zP4UcwT(Hqyj?B<)gX zd!$2!FVdmHhv)ws>0pmYJ8=I#oQ)Ot5mzvF{~Ne}R6Kt_Wc{i4_oDu%#r+4Sy}t*~ zKO=n=^*=c6{hRRoQPKWG)84->T0ihR4g9x9LdX?M@E_3cPk?_no_|ij2mGne%kll& z8);_Giu=IdH1%K0AOQ1=RQy4MQpdL?hCkNA9^u!S4&eNKh{b`}7A68ym5G|%tG^Un$R zi2lU$pubrE_Ob@hAK{1SZ&Ll3+slF5e`41(gHBl?Pgimh9k|A7pN92Ml3Jyig$z~# z2FcQD^kGp!QZ7V=?z=Jn){WhxbNklcmVZOWe73%0*nM98l~>nX=pOF43-tygU*|5l z4E!?j5uBo^sW8F3yoEi`tt^zwKHpu0ofVY&r8LeiMh@jvDK8975{nr5OwtJqiOq34 z$>x+J1V1&l?k@u#FWjm;0d{^}oyQvpkPY%X(wppC>_ya36eb}^uR}Qc&p~yxX(*!) z4RIb+|LpAv277vfZ}!&LMB2D?J~X#kNa#;Uq31 zA0T#;u*uPaGfF3TXq|^e4gjP}X=dseC51L34=oO%zz>nlt~Sk`S)GwnRdQFg(^LNV zGpTMD?MVBiO~aZ-q}8EhtGRH&MyOsLwLiu4nGclhjuK3}2T z_v2E#-RHC0OMjwYS%9DFJ^EB{({JGTPr&g;@LTfcMDGZVg3pQrE1XUlLvA)0$jcI{ zrr2Y3$pKClAx-;b!$T>{kiix$`|Q7GFN{BjM!aYr<^ig|1!mg6p^ur;fa(;Qi)9h= z8-tjStvkwB(N4lke%xiO@e8z@GM^>Z@yv{LM6v~H8H zc99Ex<6tmmsTWzJ~v z+%apPbq`*{W2Y4FEqph8AO3}ZI|JLFchC6kZ+}ZP^h=Dz#h^3n$cM9NBq+id(SewF z@#7wycxMG@3I;sr?7yI5B;f&Yx6zDMJ0Lj?WK-V&y0G{ zP*KbL?FfG&sDU5x_mz4ZTo$vVrliDI*6PhKt}H39@pvj%jXxD76OO$kKcFQlDU%w) zfr5OF&>57Okn-^&Rz2v2T!U$HF9lt+(|Osl(n6cnEJz}1;nhgegiT+laN?*O9eKrN zv0q0W2?p45jaaJeWe%TrM!k2auyy_gEsJZ=mkR%a82u>km9_cu3ad1bsWU;B#nD(@ z0>JjO-?2yG^Fq07>Q(NPE_S}gpt49U;=mfd0rmxS0m(Y?mCYJ^3?!6AiWZ{jgLGxr z{&iV{H?GSZLG4F zR7JDzr;(W&2hEoj9yF1409qq4(p)l|!pnKhZ&RV6H71 zx2%=sj;p-d?WtT@AsB(RI8^O|6s>b)F24>bG9vv(`4!tN{Ta0OPhF~EiG=0&&kiu z&n^}gJ62+~5_1#w4{^lWOh$xINH{A5k({aC=Tn#TA6}Z2%c^Q>D_O3gyKV7V%*eK6hlU1Pexyt~YwX8fKpeXt!cUk{jnFJQ(V zDl{E~19KW4C9wPn8>)!z0Q!+hs+qKZig7KI%8CJcJ{hspc_;XtnVb>Sez*X#3b2qw z&<|JeLjtvLWL~cTwXNHb%j+5;qvR%CDt8l{-%CiT^mqHKQW632V0)L_|9q~ot|lo3 z(Y%tMAMOG`(&5Ne>=C&d^Mdv$afKFAun3tiAP$X$D_kbER^xIaovG{aXZPR#nOxof z`TLM#nRFNGxIJ2js>QFs^@+QjS{(`GIDi+sohsSfC!64i#H8(!v)O%ezmyG`Hq8K; zwt+&wX#oMPSiXK$wPa?+qD?I=n-*2fT2lGX8_w^p>ig<_n>O9|)xN6ktv9TpS{_0z zZ^YL^J}C(7M&2t%A4Plty{2NXs8h%H5iEZV75HxILLb9Kr z+58sw&ldOp1&iL)`{zV9f`*&Hqu-VOL3$Hrz&}WDLfzNn-tWb|_om!C9Jv^y;lXo% znR4$g+`C=edw>s}7t!=!gm!`2Bk#339@Gz-Mn@ZR!n z)Udd@X$&=VEgurNW=U)-!e0`I7`!-Zi-QV{wm7xu;`+%V-;G*@zT^S4l3`_jD=gV2Tl30XxDz z+C{Ni)epuG90*)~`7IAWjQ7yb+kWnN;YEk~F|~%&B-q=a>4vOkqpyS0duSwp3NGsp zpn}WT$Kuf!Ur;}Qc(zxX22aC}L~OnNJ!BXv}g z{k@ogLP;*29vNisN*9qvFS)}`_%J{WB&QCrdnk~oOp^@FUW?dQ)CGM_lG1)^zs|2J za#SXzj?5@a%Y3!6xY(aVUo#f^D~dC0c@G{=D#|LZs3GXrKLty(gQ2QyaAp z-#4^OQR&d~0wJ&>5CwWP2Pu+8`()@7>t29XLd%3-%;0yR&ADA@^R=p?Vt=-PgeRd* zn^&}11W*dy0uuCo1A7y#Q$)f-sr9h}we#`PGBqxB#YxK(a5DCoMZ=1&!%xHtLZJvf zdn?f5PfDFTqZDufEgFh4_@67vi!MuiE zxfJ#uF8@a|VSjyO4f}xK56VG|S*Wuz@@PSAm&`~v39CO179=>*U`XbWVH?h~xILs= zF%h^-bIhzG6)>9VE(9^+obF(`BUs$k-``anDlV(<8Cte-)v`fuPO50M=Bz#S)RnpU zt^TCFd(*DmylHRx4WQ!^X#jB$S7HwNq_trOZ9(OmQ;j&am_ys&@}RANGI93jn$jVP zAd(3kf1MJrq?>G2`J~L!b^!EQh@qp9nMl*;Yz~5%z~}&{&T7AtOf4qZs;uCCq*lS& z2P!0sAI(i{$7<>=Inq8H>KUjR3yr!xi$aU*di(0fLSwFyMWKB&X0*3=cDC~*r*o`f zQA1yM%V@(QhhwZ^tg*K@xTIm*%VFUaP8u5uk#^!j6@QNSF?3Fx&^1dRBxJ z$r%V6E>py5xeo&ggMzfu2o-_0$SLipgu5rBH`uB(n~g@Z!;F|C#$2Px3+XeNh>21f z2V~9gp@mY_Lop$sYJk0_-1W7d9fy|mch6|AF6`*(C~?5tVb3dNes-zyAd>X$9-TL^ zqTm`MtC+h$`CU_cxVfckjC4vD^tVFjZysqucm%W!i3$Xc&2I}JH&DDv%G<~I3TQNd z8Xo2=K<}wb(r`EyN}YC!dPX7yE7ajGaXO0OT#0QPyr;uTrDv1F&Y#PSmwK4hObpEj z2Z<0~;1KNFL$2~rOZWVYzh`{#dd82_<_uH}sOzHhTbH(+a}j!RTBMTwMZ}I;5{8>BtZV3n*tMldp=56$KOprqLkp6b zg5r_P6gCGk{8>od5pl~W+)#J#V3o?YWt-fpqlfAtkKkx<;@8DpA#Fn=b0KP|I8*IPQ&r7W4XBxGwfxx$U?VAnv`2+?r|mPpEVYZE*M zFGNSgJQXi^pM-dI@r9@F zZO$8MEErk1KmX`A^N)mwmO36CTk`XQpm(SsjhWt-rhTV&gkgS$ivNBwlvS2!OmL0{+!ch^o|a76ISK}np}aE z#77EJAl)z*B6pcx(1;KONYn^dVJ9}#Dd>9c?QNO!=g4zrKeO(UO`C>ha^;_lr_Ji! zn92_Fa|>p)3@tV*Z?L@H)}9uBc_->80}Gv{a7yjr7O+o5B_fvwD88CsA`!U}VStQA zVJZ>ZmdyqtawlcJg_DiRoeb9$2^8TaJ};sIf%j;Ka|i)sx(u)v+Z*$nJ;BfmFBG`h zNQyG-aV3w8R4s(>$AmFKW=i~Lo3`xUy?fD>S6+Gc z_m_XmaQ7G+YhKv0dw0viX0}NAA#1~0sIPt80Pi^b*4H0yfX0pAI=AKqhxo1YKW=b} z-#TC62H5mbUZ-`FmnnVz7nx8de@Bog%$21@&Al`p`c55WCo~{SXG^v09b|MOOQVhK|Cma+ z&S;Yb=jdd<0>_bn?om3GM8~-k40FiaS3NuuH9#g!-dw>fk3X`^`b&L!pQ| zEh&#LWui|xl#-MP!9WJ5)7qTau4wzE0kn-j;tz`?Cy0Fnx@zJhiAVrn!ORIhk0->L za!Dmyq1;x+Rxf~M${|zRm9Ql{D1zxfgO0ER*+D7<=73C&jgvr8B!e>5? zVS@$?(#a{tWQ5h=OAtPo^Hy^HaL`wA83#V$0}jRAFp3W(xDk&)61BTr6#3Rw>GGGC z=9T0*aTp3w>#!r4DyF5n0SG@Zp)As-suk?kU`A?(2FhMzNHi94;eg3ag99^zNI%fm zFlT6RX0X4gs4u8o(eJIc<>ZyynX%f&owfG( zqt$|(b6kpUcR zmi#AS2ZIVwQz4QiliX{e1s9>cXT{YtcQE?Ap%zbnuB3@xWPpB>$F04@%|8U)he+X^D>*)Tb>X*@fVOiQ+3Qe(Tq?^_TaRmGzbPx23KBlq_Y$x;)yiS19Bx~Wy*+_4dICiX93+?*5tN| zBZ?l%B&5aJ^jRSKi^vl+c97=SJ2pztVx3)D>d#G{kF+gbe{cT$duA_AORvxMR$J|z z)~}~v$L`hE9-rUEb`loPW+wDv8F&iqF@d+KmITaiP>Z_9B9Hh&TtvuxBi};v(N7duI>t-5~^^mz1r2G zd}nsgz=D-4^D0Y0g{H_~aN^{L7|Co{A1b7f^yEPtb4%2HZYev7l)xnft4c^^@X#@? zN`^6-L1GnqZCb>X8Hkvo9x(Wl4?v2vg}-E3R1Ocv62HHwC@05Y@Q3^jbu~p5Mdcn> zPGOEc&tk|nWR<01K}YB=Po^pp*DZU_-+rQ3nbyZe&xrV!n zeZFFc$Kzll(Mt}mdWqjYKGVe?HJ;R6G3c)xsOUbawZ67G6bRJUDo0N0su--S9IWU% zskN@ICKRZzufe*T6Zx3;h`mb-xVaaSWE-5EHib77fS2dz@dR-$Uqy7LF+)zsPR){& zlM<3wr3+*u$p$tyQ&>`x8ZrtPX>Sh%usv#{jq3Kk_TH|}KwAL8o_uw_+R6$wBzl@9 z&77S{tG+u2*ok*&AiNOs23iuZoD0-w>zjiRsC_rYiq+{~U4I%FXlxoBY;xN=a7xzA zt{#8CfSZ-?4-7Ol;f2*;ps{&iut|B=*5S#|_x#l!Mp1rbu&sS?u)S?CP~}}zQ|hgH z_@$#TV!g+%eFCR_ zIPgw#A`*^Sncqy2-3sGM07PQ~UlDhI3@*>=zpSJr1dfjB zN&&HE%)jQEVfqvN^3uMWZtA;!{MZ1&%GR+v`C=8;X}F>dGC~`)H{pY36uFetoE*BL zFxzAjyOYdCi4*9B?fbUTc|V)Kh?ns7pZ_dHZWnLlt6vD2;*h*jN|KNhidH*tLY#C4 zKDe|3EE0htaOgwW!Jr(=A(JV<4;}i=$#)NY^;aLs1)m>dKgmm>y_2Rd#jd4SR_^s)Hc1K>Pwcx0|2I%ql9eciO((^Fi;Jz+TaYK*#Ta!$FXgkKmx#TopW=;Z1G#?Mce?l7ty}TE4X2<_^r=JZkQ*2>obZQz{*fw zr+02x2>ND`zhTCm!f(c|5waMF%#TK>{ysIKmeI%fDSmZZC6~(!QI(ZnO-i zvt;GsU2JC(OPys=PB#23DZA98Tu8IgD&_G`z7;!YiI1eyULO|JP=jg!Lw&qc`D@43 zu0mx2yFb^Ie{~1adA-j4-%T?)%9M*6AN7=GCq18_oNSquitX&3jf9uTo50H!0ZAI+#RKoWkd?t57}oXx zTPP5sJgYn#xTDlGf5cpzb6u12XJ&1>>P4wP6AzgcZgJ0ISg>qYmsdFkY)%tfEC( zE6~EWvvK7@ffsg1!t;a|Ro_x)N&kXDiff?mO z!pvh*J-?GL!&t#9LEX8fAtFd*lQ_=>Yb6HD#2)H6yV$AZ6uP!_eDcrsEfz~I`vB45 z&uRZheQ>ASV)E?_V&~i(+?kJDin;kagUWjX^L_jjd4)t#@$=I}Uj;?R@J~quupOxq zr`eX8YR|nRpbiE7QYn=WHtx+SHh=z-*_pk!kw%7oTjXkt)ZRwWEmjpsMT_LH$oZIb z(3F{Gz#cZ%DM%kAeq_5Kazj-K`1qoGZPq(2_IsKCsnhN`?I~_ivMhGHg&k+*rE%4Sw7JK2RtkFqA^ zZH4xL3}JgifaICsWs%vX4l#7RbEzJ zzSnNK^RCmLdg?U(fz7I9DX*|{cHC<7kN@WAQCLK#K$rn1TtMiogGz=jJI(OovXasl zgc>-L;8Px@O5Q!nE(DkdZPx27_5;{x@hdMswdwXvpz#<6HNT(<*o#A3JjTRxdfyTs zNl>-wxPHZVXkuT8W5w`{HigAJz6TRQj|Y0nvnqdy8=v@ri5n4u7dC`N%8#+eYCip> zU6Nl4uBIKK*(FLuE{-pe9WBAO=q04~=}YjsXo*ixP~ro<1Uoo=37neK;NWLYU&3&T zUPAUyUn0Q~E%B!llz5X$NS|Wn#Fg;CdZ^;#Q;;l`ko^-slNb&N?v@pYJx`3id` zN?O938b+mklke}_w=e$wTa)j<_10T)_nANbJ_C$IyLeF(0#|IyloDH%E7+E}5iBXwaJ(uT*cs^l9NgR6MT_|3KdSjG>IFct&V+@=iamg0*jBLGBinTV_KOzm0ChpT z9+Kur^RPX!1UopZq&3oM(k2+&wn*no7fKgPmq}lfu11W78>E{M8RMJM-O~MtNbv}y z`6s2PAk9B3{Zx7pdk?>s-as~y-%Ed#-jhC*wuiTV@RxW0bnKnC|KqoBzW(YfFaP4_ zFZ}GeAOGm-AAIk-N1izR*rVS$@X!PI-Se&eciw*M&EL3j-*tPg*?r|#FW+^^MLRCo zw)MPo&pzvnjT_dlJ$3cU<;#{XUbJxj$nfl$gZ;hTGs5ky&4|=oS5pNeG|pQ)itI>i zoQu$dX{y1#6#@fP0P8d_avJ^r9h z>_PSb>bw^<-vy2Sc6JN9nSC8`qONDxq1|iQRqRT31-l%3WtXs>YzHL$ZR{)TJa!H{ z8~aUXu+wq&eH~8Zp2AkM6>K>>i7jPgY!O?)=CiqMn9X7{*#PTD1h;P1$-?mUY=t(~ zh=UvT5b$eQCHPbsD`g($W=`f{h0KnC4|$NUa}hi-i>0$P__x5*fwro!9E?k!OaGGo zDg9mgSo*7UTzX&ni}YvdPtqUYUHz8yAK*E^kzSKtm3}3?EWITCTzX#mne-Fs$I>&> z)6(~)?;%pp5pb%7DrfNU9*&Wg?(#_J>r5mN|5wGW3=^F65 zs}SMla_Lg(5^1Nj1J?9y(pRMOq?NEnE|I|Q$;j{}UO{1sAag|g3#JJ&PyCyg83v5W zuY}1L@SCm~CtauCldn-ZtM<|>{<8I*sGMGp{`o&uj`}*eZTe09jlUlMJs#%Cx4AX@f6c~!s?s9ja7d{b0)12uuSZ{kgSkAo+AS-y4V;FSMyta-}4!J)zEzX@N5 zn0|d|aME}E=LyT}PoA*y|IGq=!-J?8-}Rr9;n1&B>STSLXp=gn9%%q)d*|Y?-I#O| zti7iopXlk*nbJAn6WhTvE(QO%3cTcc@ReJ@VeSIIc@VtkAh^&`@T6zJpPmP=`W5)r zZ@|O;0Dks9c-xBbvJXFa@7+Hh`~BO$`|TUA|N50*{^G?Ke)`SPaJye zk?(x_q5JQ<`&-|<^R`=W`o<0W_U^fQ_t(C9*{+K(y6}SYw{AIi^I4lVp0<9?sjF6= zyllzh(S`Fy=FA=%?Cb5C(b3-Otpe}*Qk*NEU&X#>J5Mx&^x33;{@+xNFPbu!CtQFk z_P-Eb@Mhe#eX;NVrC*_0!E1~+bGz_PK-Oon(Zufn!Z+ZYZ36?_ z1_rnF_ir2I_UKo9Av;g(`C{lRmMjDbnOcXMN*5buv}1-{*Z3;yM+4Y<4J-=a%f#x3 z)r)j@6z1~(skZXaFYEOE%AU4I)AdE6K2SGey~u)%5H)8T<1l8_HeN-7dl(x7*@UZ} zEq$pf{#9fNH1%&CkTD_{1`SVa+e91@+@|T_#&|fV8wj>-+Ci8IcP8A_PFS-{KoeFb zjPZA=^NS`8ukw-F8?u&wikKf%m;|jH{(m(UFgwyVGsw@c_FU~Mib01~DVEAkpjd$` z;6tY()EC2cCVXH{2sQ8#mtM349nkzVJME`$$vcMkRvZQ2hQPBsUB8dNF1ss zwd&6vPxmL!N!v60cE;LnAZB5BVq43H8}_{2xd2$}TAMMkO`B-5*q)1#JwclYXfCE1 zPUTo9_Nr@L2C7Bho=BS$n%LyHJrnw=wh6bw>Dv^u+ZqgDW?RvA zXu{B=u?3xPWvuNgZJ3ibsY&#_du_(s;f2Tcqz;Fs>!-tRGoPTJ@Zu0|9Il`hE&C=k zx=usgTf=+Oh8rffx@v8g3e&X!Xkweiu!5u+apT_tm!6%So|cm{ z&G2Yl8Lf{3Ih>QG7LTJ48I~j$FjtV9qWVTUVXHC-5Mt<356VXQ8*d(ehuf7$*SELp zf2BQTWj&?%!xwHZjb6u>;ph$Aqgl-|#Hy7hW_4D2D)CU}qG2gPy9fuJ2y8fsf$(On zrc!P{)>Eduj2c*mS~<%&Pi-#BIavmFEQnL&W(vz2dJ@oCS|Ec9@SOy^UMW?8Kj^Po zomz$9X&Q`l>R6l+q?I)WqF`=d2*bcJ{)Y9{RV5vTj~l zo0X50kI<(K^*xTmlT|b6pbmtCInE(bg+`Q+tCv@kH5FM*_Sxv`JGB z>r2M)#*NgASbNm_xboztsJ8q3c5WYrYbN9ahPWYV{3Nyuy^i{oV7J`KPf|V^hRLSk z^H=1pq8w}BCt+tGt{k=^1F+{LD|ZjGUBmo4*vVikf{9)x2U?)v3 zAEr`>Wi?-~OSMl2MO{E&@-S-{<}1~@M2qC1m?9_@8;%5?x>N(96cK zK-f|t7Kjk_5-tS(qvJ1jmQEVKl>ns~MI$7z6)S=L6x1}V?#M)26L^ZXMq8iJ76qO# z(ytE>5Az(*iFj(ifH4(x0;9OfG3iiLc7XP-m|^(w$A@pHW(00Ob<-}B`W)~p=C{_G z3!_55Nc&Oy7v6@ZF9!lZgOx2t@3GygrP1@TgXgI&ZXn6YS^eeN%fz z6e|c(o7Ud`VUXv&qBZnz>+t6dq9sHUKc+z(_oN5UWd~#dGuy9>va#XY3EpFvC*!w@ zF;M3L^_6e~R-^ZkHtFe^$AL~U8HgAJF3}Rrnmx%cs5 z8tBh=$QH5(gH9DpunkP7}RJuEeRQ8h=z&&RG36vm}4qBfK7}9 z_Hi|k)S5t*)IVTmA{ArI&3LJhXuUa(UKnWhM8_Z-yLpfoExB0&DUcVnDI@?;iCXOv zSPV@iLXJ9_^loV=5_3xHody%AiuMcz=g`Ml@EgEIYl6x>@c}jl;^H}kW%V zlCUoR(^J&cm`SN$0o|dZG0J`!uvn6VKhM_Zs740**7p zN~O+QwcjyZi|%Xa)*h!dRCG+komNZEB;Em>>s&|WIRf|USDlv#evYL^lW-WMZ%SOWT;fxv8{t)0Xo#mX>ZjZ~kn%efE6y9h!xHdCR8K z;plbTjn@VVI`zC4$yAujLIwkbbgDH55L(&j)mnWgpikL^>R--nuxp@L~^mLxja*O$h+o#^g!5l0J zOppyJL(`N{YtTlLl&llAp?3I0HK;I73xnDRjdZ82J&^4z5|76Cc}!l9@$%zQPOfo& zwjVq*=6{2dB%vPH)

zoG+v+UCI6#7` zw3Z57u>B-$3?FB&h}9ZPW8BIW!-QI1lcMPffm$|GW5M`)*OVEZewhW8$ zi0T@#*$T-Q;}Q3SYVFV)p;4R&7x7$e)&#V9g9%HtUOudH=y>T`m&R$4)J9Vs0n!%Y zaTS9|$CO$=HewU3MUZmiu?trm(F^SvmG`LvxsF|_QCiO(I0H*9O*uWw9Zx>lr2p}J z{TnWUHuWcI&fthK`fp?*?r2ixHhr^+eY*+$m|BJoVsZzHuy6Bx@j;v~YB`GY3?=JP z0?lTqbflk(_Xf-l@V_`qQv4INGSMu8MTu4>zVcHy)L6`Z%}O_McB7O&yXijG*w}q) zS8y8DauwxO(fOd;D~N}v@R6}XZA)ls6Y5eqO}rIsLTeh#DmRYK<`{Pny%PO8PW{3< zP7Cg&85OragQ^5wi8Y$c8)BUyO4N81t<(C95|pb^J5jD!nh4=K@uHn6v@5js=zJ0w zphg7w#^8!k?}_FUt(g-jScfWJ+aqa5u!ei}`KR*PNpl=CB+8S)9>LLw`^WVdvm*x4 zbn{7LA+g!5&nFEUvEs+(ELMGW)(S2C3udm0U2UdoEB^#bFVR2RiW~80PsHkijzsx_ z;Pf#$FK&iw^IK@pdUk(}Yz?7|`RW zGlp~31~yq5ROKd(6Hi%Z1*NHcTeZZ*R$6V55X zvNsNkrFmLhs8G}B=`ydN+UEpiMXe?=ToA2`vSjHKOZ0Rz;zaZpHydIy!gMRJS}+dc z6Rp5BauZkJ_z{_YotrQwz%$uuqr4BYfQC7BRg6gksx+;a)i@HhpyGq}7N}-GgAV+m1|z=i-FS~nt2k`}DR7z|{JqIiUKAIIs~;=yuzLB-CB{KE3Vs{hDNum`fu zsi|2xndwP52K8QAQhG*CR!XWlCty#=&QTt-TC=jP*6fS$%0j-;xn@+9n45HZuZ*Z` z842kb=FIeDD`L(q$jnPl%goKlNX$&j%#wQwxUICeqN%bzXJ{y=-rro&Tl#>bAS+}} zO-fBo%jUM^jO5HryxDkmT1r|{YIY#2z>#3h&d$rrrY5uV^3n82VOF8hh_`(eMI~8j z#`L`GwA74bqt9nd&OkkRX-R2WB}EnG(1}#5hzX*rm8Oh#D^yOVYMu8Y+T;byrQvjMRS|YW;Ww>Q2T72-P}Ce=(o(8W$|-c zzwWvK6PpuWnghV9K@Ga!m#L=g()&Y8pdu5#0-c`a@AHgJe~%Ep-T& zlWDvV;$02m$wUiXr9~2);DoS21ND0C)0`44D+{Jn7)P3$M~oAJzFLP*|GcBDrlzcM z)-1}`u3{cB^Ko`1>7c*Vn?rjLAVS2#6mjT8I7|^}(q}|OOdl5g21xg@}xU9OgaJGBc zoVamgqIuXoyTnyp>KJy9SPiG$5U#1|=%}d)1IDjmG58r~Y@Xx>jD`8aS3&i%gUt`i zv3fXN^LL8JCL0~BnANg;dCl_WEwd{I{52W?+*aQ2Z(WcyuXNqI(vhTvt^WSP zti;h&Q?YOq$H8N@m!tMv;{GBIhcU{3rnW*1O7uLyR)_5c$jMe+*HY=O&yBWO@2YI6 ztG49l7TCj$+*VWVt~4hnv^7?>=vAwarzplQ%4mGWfe2lCWl&MbB6cD?_ z#Ew7x^wTUwd6!ujqBJu^iT;@V0=X^dd~sd|`izjO;N3oiOZ8ExOTmu8i0PE~&PsPq zVP1*V(d4Ra9H@8X)v)754Zc!uT1%59Q@JR&+>X?g_37+#syPqmiE(}%RFn8fVGe03 z6RI;#tlh)&qSd|^{l53|D=XpxfIHNGE-`yiy!&J6T9x1ThnDkmLxxd@q4OKLy0TfqR2A79tBMd_Svl?rH7{$JwJJ-_oE`AvSoycA`n;`kZTTN#R4O8V?uVaF zrsN10CLq8G!X^{(Y(_6QC7wb&XRj%T!m1>wFlZgRF9~9p3#IM6g}*1(?GW1bqnCZ7&7j<@rJ6$!^tH)Xw!0FZ?RY@Q6G7VpTthlcobR!5D#k#TrPql~(m;l`IGt!t5 zQ8JQ@$vUP|MHnW;C}u4bia2SBg!Ea6FQ_88db$Ub5LhfJ0og(Xs^cmkR0pF^pibN) zoX0g9|7<+c$HLyqN)_*wfl3?}b@&|>z#>lRquH5f zcXyxN(?b~SXf;f$fe#s!6rwhsPY&pACkTFIX3#^sGo%R-@L zO-&~^h8Gl-CZBuy>8!xNu(FK0y*~0JqCp;!YH+>;*{(2~<<1J5Nk%}i0>tzo_C+!8 zPR2x#mmnIN+$kXp3yN1$$X}0|01b4a&t6r)*Q9bi{vdoN5uk!*Grg(Iih_?KN+Wp@ z``DRt+@+4zvWoiL`ucgjes{}h3s(-57Kh6!n&?VjrMuwJF1 zUeIL8^*Zv3Y&o{>CSRw;AMTpn&}1?D5WTb@+uqge?X*;eXACR9s509f`Cwq&RB5q0 z3W(RIvvS_W4?y}%d;}3%$gYa~G$D$agcMKu-B40CJ-Pr{DT5@y7 z`u0_9U46yLto%=wK)&)Ob7wl8S$}4;mG7|GjmjTcKH;_#Cm(kq5;e)VvsF6G)rgH! zSux-f)JnuwfN!{@hML2p@e~!#iN!`mRT*A2)78YOtMf+EqgcjJW#v$L#b8y{P`RhR z-a{|`zNfFYrqAQ)ufcn3ZFO~^uDY7?VVkig_wl3fNG*gUmuE>Q-yCpcD9@ygg&@O{ zEENO~hEnDvos2zrMYb~Bw4!@eL9JuU((|t}WnXdOiUwAZH@lJ_-MMy`@>zADs(s-6 zHOktZzVc80VM6pyE{!59+&jQWK6pQ*)B*(1hqtN5S%O9g&qN(UIWZ#k8P+t_&D_QI zR9otXdgmw3aITv_v#sxxjibSmp5}a8OHP|@R`(jYzGB|**KX`@8ohbm{yu)r;PB0z zo4QLJ;3wF^6unD^eCU(5=(%CTMgvdo*to%v+OgGYEiANH3eW%h z-}%wAPW$a|*Pb)p1pxPSD|hjS#}9mE8(UH}d%?olvql%p9^c*zS|8+@JVWkB2lB%f z%y6XZ#%S=f=#=ZaY$PzivIg=8$p?7GlFKh!d=yW-|2}KNvxvXO6i#^ndxj2_16x)w z*CvEb$HNGljx3;*{8C`ZI^m)k^o{O}g_A{KwP@fsF|R08*8KbK8^#|`>Qg${_xo;% zYtJNIGIYR&_MEW**T8er5C!v*#t{FSS{AXc!Qu(9;%H3l6_Xy(ik+~89?_eUG{=R_ zPMJ1rwhFcYoIq!Nzv#Q;4!rG`r0mGUk-zf0#p-3jY;uF%YGHYAm%0(_U@jsZER$9v zzSfyI)$0qp&)a+84zDjkEfAm&I0MiJ<}!UyVbm6)2PxT@ zl^(?T3h?B;51o4ILu=NE_d>bP)6rNOZglr0Bz=9X#93R?@>X$uQSVyiB*R6rC0NQ6 zgI0F2p+)JCzuwXHPV+aIS;;jt`)e(-N6zi&DsAsIT)={npJ&NRPfq0I7YEZ^EIbmDK#G7#K_|K3AnK3usU9im zVi7lqCI0#zL;xZW@A7fw7%MnB?xpxi=akQ#i+z~`lAYhf?+`0V(m``}G}003rRYf% z?8uqmgC)Wmg0>B>^s5k@XnX^ODB{;CkL~K(rT_6eaN~DWH+NcXpIz2-se0$q9<+0| z)=mndp(Y*7N}JFQV@~7bCX^B70a#-n`c$tT?dUDgIs8fWQ$OB8KlJ*O)Cf?~E=;KZ z9pwRKLdH`G{8d-J1BYi_xV?VrDA9&9nwPSROwu4m$Xl^hI6mFZ2Oke z*PJvuE8I}y#qk?B0}ExIVp&DoPoU`iP%_9ONlR(qOa2xCqGFHzXRFc6JGEx{w`lYh ziP5`f{M@9pw4_0LTRF|Q)94z#8t85O=rl!Qk15yDZT2X=^RFO$RkjGy^`X@^q%|Sh zIpNzy^x(%yY4?3`>APaLF}WgdOPTy!M2E@78jRD3IZ3L7A>v+BOf7RXSXa=9@LJh? z2}RcWF-6u|W4GM4|N994wN8Y&3Rvt9A!b*_Lo5+tyOd`U2y7F3jcnMu9Lp0BzYo84 z*^4Kv5x=1mu?W^Np0GjW_lf;JUHuKc)_{njoINkf{{rzUc0nWL340*!GU*qQ^RZKS z;Z(bV(Nd9sg_G4R+uV66HX*QFuHri)v*kjR)vPQsM4RutRPYS{)q=`1s6;y-jI8G` zfOB}lB`G*^f^0pwuE*+!kqbGl(Jrz$dnk*uq=)hr!TOaO<|Q_#R+hMGQkxSOw(`Nw zhV){$yEwg}6Lr_}p2!yd23Gr=a3)0v6?J2wr(E4Ktn}azSL|xK%)9*p{_utkv@4G2 z_^TquNUsDLG-2QPy^+6bIsg8yCZZFyE<~+=L9NcmC}9m$A1j?+=V ziV-NF)MQxbC+D_C?q!kNn&JpUP*;@-6R%s@#;vX zYy>XdVJD;qikC{L7CBdu@hgUEQKPCg~L~&+apfDx1*or(VR*UnrbB%fF z)?$eR&s!r~%jeIPU%_&29!_a|zh%q=Xh-AwEi$T+}g>&LDKtxN5eG1A@$Y4PlqzGshIpxEW z5}VEGwDA|6`2{5<1^G^D<0Q~y|73bRT=0^QKUle-pGLoyccU*zPLy!d_&t&5Xk_@| z$aCt*;QnmBF|vd|g!?yxI>dd%|62U%`hF= zx1tP)N&EP@?0a%5Vw%F{bOkGkW>@hUwce<+5 znhf5~3Vv=`yEA=3Wof#z9rgq!UBS;`FUrUTFdg?{D)(V8g8h_k+s6O0WlP!O#dzw8 zNDjLN5lKx_O}H`v{mW#slmYVtSrmm`7FJW(Wk)IFG3;?NpW{YKEK09t@FH!Ln(T|z zR01`~=9l}Enyae&0!5)*Tal-AbIzFqhH6K3AU(Av>qKxO9IeJBO`!mGGK^PT*0jqD zFmDBzOMn?JUBxeF&tV3qLgMRI$2Ajau%P%zB6pz3h>ghcG`4E)z$M}~xcOLE%~)+s zv*cun*-LtSjxdO*`*C$~-yd{1tk&XU>tVdC_|7jcw&WET=UI!2tn_Ii8^(u-3cOaP z(?#GNWJZkfj;{YS?_j+e?~oIm1qDue(Rd!_8`-V033REgWyoM@JY03y#(&zsrHXKJ z1>cGhpTq|d<`i7e===51ipI|eHw5Kw_x^&1eBTrv#dq=jm>ci>AJ2{bVs88-Vq<$C z@nuVaN3(IVsDJ1{A#jn3(bUs!*f43tkZ9LvI39I*=-{POvz&yC8WRn%J^l3sMYfWp za!cckCCiH&3iI-e-HD-!;4p;>mX7k%*iCYypaJ9yII-nMBq*Oa+`^!*G3yB?o29gq zhgW`KVSa(VFkfzLt*B^esiGO%HlIGt zR@(?c@oV16cFK@y^z}oG*3O+}Yx%1;ijz)xz+H! zk+1w;$n})Z{*zo!IX%)HX^p%FU1Uu-1;%6Pf3}FEDIa5yR2E4J24T1J`Bo5AOXaCAjZhXXw*nFt2_V+4-f<4ffphP zN=#Jr!I=0$d_WCo)X0Mg`2AJgv$MM_U;@5KHq+hPU41Uq=l}leIbV*_3W!77KyMHC ziy>rzw9Ab+U;t@WLTT1Fl7`ICPEzbYFW$fSHGVyOG zpQpVdcK(k3?W9+v028~AZ8gH$;=pM}qEnBm?p-^r0<1uuIm0*Dld$$p*w6d7dU*$2 zR~ane&Ai_4{+ z+`N+XMWhr$L0DF+gh?*eEXo z(Arry0!x-%HNmK5z;)Bh`(wC1*=Ry?|2PT-_}&UMR({yeh8Z6 zMnvy=8Kmo@aH30M&^Ye#&`|f0jCw-rNW-RFSfy9#wWjcOV!qSQ9XjRvA|mI5e+jFM zs5K@@KrLb?Cxah)%VbsxKd+DN%1aBb3Cp9|DDc0G$KQp8$xRiiXJ+Q{R-q_NWY#Nqp=9>Q49qG%Jcio5xBctgJx_rOc&AlLQzxARJx0G^cn zlY@knmUT)3nG$8u8rkYza0yjkfSwgkpk+~_oLOs2cgJuthk+$p!4^|6DJ zdz*DNF)?PoGUA?=)&Uaj%9sf)d7hLG+GTZCO{NQc8@~07fIdehu<;-rl zjb+|FKJ#Avy!fo2H<$Q%R3vkOpO>iG_v+`p<8A!!{k%WHZn*sdyW#fjBcEVz7%xyP z>)k91hMfo=yAgE*R_2nYk>rJPJ3rh(5$JP6rK!40V`_;Wq9{7q4rkT)deg}7TWmy# zuDsTW%=+|o_jnbkRE+6+{6Tn@l6rh*9G*{;%xM&lqGV-;F4HJpO0b(m@y5Z~ByO)# zcxPM!lYrabPO1lU``hh#gzc|d=MSz8t!A~_w1#Wc=XZpHnB14q=X=WuoZI&6Cy^3J z+CHeBS{KURww27jw@%B)_#2teux*@|eFwWQQ`t6gy7rwo1^nYUIr|V>%bYuk6lqt~#ONj2_3gq`<#HjFgaw(A+S*VGk)PHCc6C_X@5~)=XEh^KWvv^HX4ERzvVoBcb#KO6($)v- z`qm|URc{}%OzFGY!H;$H>xB|r)~kk6k9=X5Sf1HxdEm)n--G2FI>gB>%|;*;U+MOj zxY5f?vnYjQILoduYlW!HnQyRCJFnzLccCDvsBo-Ft1Vbw7`kG6wOB|ya*>KPn$~MU z$cUZKS=ua!Yb4XkpvW?9vs{valqW(dT*-7kEmo}y2x?1U_b0;AN>;oEQx9^Jm`4x= zO0}a}0hd7pN#03t=wp19YS9{X`vts>RVLW*aNlF4Yf3xOwX4FI|6Yb#>M;FCVHEroDijD0twTOu#7)PU%gXO?C zmAdKPRLtJ%&L1lp*kKe`JE}QjFjq5I9jVL~6rF1wtlZ$Ym#x56A#MW&tN4bJykf3e=xd_VQkc^!NmjAub=D{z z03V*_g>Bz8l^(xxbIH+V-e+M@)2AcF5@AJ;!g9c6u2OC*L$+MYx}kwJsvDZyE(pJt zcgk2rS?kt~K&-2QX4ci_Xs*@jdj^Pqc@XBLh-;p$=!VEM(Y9obsS8z6Sm+AKOuNJk zpYd{3sF}iDhFl3*G-bYL@TGniQYR`EoEHl00f2<2*Dp$MP*!b7YRMM(tz=m3T^1+; zXE_G4RE8)7QxFO!uaVa&8W#)T*_HV)$K*7OSLq5FnqtV{B52MMd9j0rIBw~oGNi<@S%c88$&wv9s5*0DH$b*tfwV@(6njydTf87c!gM^@J1T zS@4NG2@a6Qa0bSY!2a=lu#J3^Jp|?va2PnI8)K2ue{onbo~b$*c2Y>zRjU;}|Dj#;KG-#t zdDu0v4{^_`an8a)k~W`MImNngG<83u-qzZ}QuUnd>Mk$$IM3P)csL^z4|30C-<2XS z-ao6TXaB!jMW6OfDKwLp4AsW2r+#WA{Tk+=X2~LeRP75JTfe02@IWedU}ykVQ8u}c N4677Vk-~wZ{{hP(K}i4r literal 0 HcmV?d00001 diff --git a/examples/data/fonts/firamono/OFL.txt b/examples/data/fonts/firamono/OFL.txt new file mode 100644 index 0000000..029b036 --- /dev/null +++ b/examples/data/fonts/firamono/OFL.txt @@ -0,0 +1,93 @@ +Copyright (c) 2012-2013, The Mozilla Corporation and Telefonica S.A. + +This Font Software is licensed under the SIL Open Font License, Version 1.1. +This license is copied below, and is also available with a FAQ at: +https://openfontlicense.org + + +----------------------------------------------------------- +SIL OPEN FONT LICENSE Version 1.1 - 26 February 2007 +----------------------------------------------------------- + +PREAMBLE +The goals of the Open Font License (OFL) are to stimulate worldwide +development of collaborative font projects, to support the font creation +efforts of academic and linguistic communities, and to provide a free and +open framework in which fonts may be shared and improved in partnership +with others. + +The OFL allows the licensed fonts to be used, studied, modified and +redistributed freely as long as they are not sold by themselves. The +fonts, including any derivative works, can be bundled, embedded, +redistributed and/or sold with any software provided that any reserved +names are not used by derivative works. The fonts and derivatives, +however, cannot be released under any other type of license. The +requirement for fonts to remain under this license does not apply +to any document created using the fonts or their derivatives. + +DEFINITIONS +"Font Software" refers to the set of files released by the Copyright +Holder(s) under this license and clearly marked as such. This may +include source files, build scripts and documentation. + +"Reserved Font Name" refers to any names specified as such after the +copyright statement(s). + +"Original Version" refers to the collection of Font Software components as +distributed by the Copyright Holder(s). + +"Modified Version" refers to any derivative made by adding to, deleting, +or substituting -- in part or in whole -- any of the components of the +Original Version, by changing formats or by porting the Font Software to a +new environment. + +"Author" refers to any designer, engineer, programmer, technical +writer or other person who contributed to the Font Software. + +PERMISSION & CONDITIONS +Permission is hereby granted, free of charge, to any person obtaining +a copy of the Font Software, to use, study, copy, merge, embed, modify, +redistribute, and sell modified and unmodified copies of the Font +Software, subject to the following conditions: + +1) Neither the Font Software nor any of its individual components, +in Original or Modified Versions, may be sold by itself. + +2) Original or Modified Versions of the Font Software may be bundled, +redistributed and/or sold with any software, provided that each copy +contains the above copyright notice and this license. These can be +included either as stand-alone text files, human-readable headers or +in the appropriate machine-readable metadata fields within text or +binary files as long as those fields can be easily viewed by the user. + +3) No Modified Version of the Font Software may use the Reserved Font +Name(s) unless explicit written permission is granted by the corresponding +Copyright Holder. This restriction only applies to the primary font name as +presented to the users. + +4) The name(s) of the Copyright Holder(s) or the Author(s) of the Font +Software shall not be used to promote, endorse or advertise any +Modified Version, except to acknowledge the contribution(s) of the +Copyright Holder(s) and the Author(s) or with their explicit written +permission. + +5) The Font Software, modified or unmodified, in part or in whole, +must be distributed entirely under this license, and must not be +distributed under any other license. The requirement for fonts to +remain under this license does not apply to any document created +using the Font Software. + +TERMINATION +This license becomes null and void if any of the above conditions are +not met. + +DISCLAIMER +THE FONT SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO ANY WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT +OF COPYRIGHT, PATENT, TRADEMARK, OR OTHER RIGHT. IN NO EVENT SHALL THE +COPYRIGHT HOLDER BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +INCLUDING ANY GENERAL, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL +DAMAGES, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF THE USE OR INABILITY TO USE THE FONT SOFTWARE OR FROM +OTHER DEALINGS IN THE FONT SOFTWARE. diff --git a/examples/simulation.rs b/examples/simulation.rs new file mode 100644 index 0000000..7ef456b --- /dev/null +++ b/examples/simulation.rs @@ -0,0 +1,856 @@ +use std::cell::RefCell; +use std::error::Error; +use std::ops::Deref; +use std::path::Path; +use std::rc::Rc; +use std::time::{Duration, Instant}; + +use coordinate_frame::{EastNorthUp, NorthEastDown, NorthWestDown, SouthEastUp, WestUpNorth}; +use csv::ReaderBuilder; +use kiss3d::event::{Action, Key, WindowEvent}; +use kiss3d::light::Light; +use kiss3d::nalgebra::{Matrix3, Point2, Point3, Scalar, Translation3, UnitQuaternion, Vector3}; +use kiss3d::resource::Mesh; +use kiss3d::scene::SceneNode; +use kiss3d::text::Font; +use kiss3d::window::Window; +use serde::de::DeserializeOwned; +use serde::Deserialize; + +use marg_orientation::gyro_free::{MagneticReference, OwnedOrientationEstimator}; +use marg_orientation::{ + AccelerometerNoise, AccelerometerReading, GyroscopeReading, MagnetometerNoise, + MagnetometerReading, +}; + +// const DATASET: &str = "2024-07-10/stm32f3discovery/stationary"; +const DATASET: &str = "2024-07-10/stm32f3discovery/x-forward-rotate-around-up-ccw"; +// const DATASET: &str = "2024-07-10/stm32f3discovery/x-forward-tilt-top-east"; +// const DATASET: &str = "2024-07-10/stm32f3discovery/x-forward-tilt-nose-up"; +// const DATASET: &str = "2024-07-06/stm32f3discovery"; + +/// Kiss3d uses a West, Up, North system by default. +type Kiss3DCoordinates = WestUpNorth; + +/// LSM303DLHC accelerometer readings. +#[derive(Debug, Deserialize)] +struct LSM303DLHCAccelerometer { + /// The sample time, in seconds, relative to the Unix epoch. + #[serde(rename = "host_time")] + time: f64, + /// Accelerometer reading on the x-axis. + #[serde(rename = "x")] + acc_x: f32, + /// Accelerometer reading on the y-axis. + #[serde(rename = "y")] + acc_y: f32, + /// Accelerometer reading on the z-axis. + #[serde(rename = "z")] + acc_z: f32, +} + +/// L3GD20 gyroscope readings. +#[derive(Debug, Deserialize)] +struct L3GD20Gyro { + /// The sample time, in seconds, relative to the Unix epoch. + #[serde(rename = "host_time")] + time: f64, + /// Gyroscope reading on the x-axis. + #[serde(rename = "x")] + gyro_x: f32, + /// Gyroscope reading on the y-axis. + #[serde(rename = "y")] + gyro_y: f32, + /// Gyroscope reading on the z-axis. + #[serde(rename = "z")] + gyro_z: f32, +} + +/// LSM303DLHC magnetometer readings. +#[derive(Debug, Deserialize)] +struct LSM303DLHCMagnetometer { + /// The sample time, in seconds, relative to the Unix epoch. + #[serde(rename = "host_time")] + time: f64, + /// Magnetometer reading on the x-axis. + #[serde(rename = "x")] + compass_x: f32, + /// Magnetometer reading on the y-axis. + #[serde(rename = "y")] + compass_y: f32, + /// Magnetometer reading on the z-axis. + #[serde(rename = "z")] + compass_z: f32, +} + +pub trait Time { + /// Gets the sample time. + fn time(&self) -> f64; +} + +impl Time for L3GD20Gyro { + fn time(&self) -> f64 { + self.time + } +} + +impl Time for LSM303DLHCAccelerometer { + fn time(&self) -> f64 { + self.time + } +} + +impl Time for LSM303DLHCMagnetometer { + fn time(&self) -> f64 { + self.time + } +} + +impl From<&LSM303DLHCAccelerometer> for AccelerometerReading { + fn from(value: &LSM303DLHCAccelerometer) -> Self { + // The HMC303DLHC's accelerometer on the STM32F3 Discovery board measures North, West, Down. + let frame = NorthWestDown::new(value.acc_x, value.acc_y, value.acc_z); + // Normalize by the sensor value range. + let frame = frame / 16384.0; + AccelerometerReading::north_east_down(frame) + } +} + +impl From for AccelerometerReading { + fn from(value: LSM303DLHCAccelerometer) -> Self { + Self::from(&value) + } +} + +impl From<&LSM303DLHCMagnetometer> for MagnetometerReading { + fn from(value: &LSM303DLHCMagnetometer) -> Self { + // The HMC303DLHC's magnetometer on the STM32F3 Discovery board measures South, East, Up. + let frame = SouthEastUp::new(value.compass_x, value.compass_y, value.compass_z); + // Normalize by the sensor value range. + let frame = frame / 1100.0; + MagnetometerReading::north_east_down(frame) + } +} + +impl From for MagnetometerReading { + fn from(value: LSM303DLHCMagnetometer) -> Self { + Self::from(&value) + } +} + +impl From<&L3GD20Gyro> for GyroscopeReading { + fn from(value: &L3GD20Gyro) -> Self { + // The L3GD20 gyroscope on the STM32F3 Discovery board measures East, North, Up. + let frame = EastNorthUp::new(value.gyro_x, value.gyro_y, value.gyro_z); + // Normalize by the sensor value range. + let frame = frame / 5.714285; + GyroscopeReading::north_east_down(frame) + } +} + +impl From for GyroscopeReading { + fn from(value: L3GD20Gyro) -> Self { + Self::from(&value) + } +} + +/// A timed reading. +pub struct Timed { + pub time: f64, + pub reading: R, +} + +impl Timed { + pub fn with_time_offset(mut self, value: f64) -> Self { + self.time -= value; + self + } +} + +impl Time for Timed { + fn time(&self) -> f64 { + self.time + } +} + +impl From for Timed> { + fn from(value: LSM303DLHCAccelerometer) -> Self { + Self { + time: value.time, + reading: value.into(), + } + } +} + +impl From for Timed> { + fn from(value: LSM303DLHCMagnetometer) -> Self { + Self { + time: value.time, + reading: value.into(), + } + } +} + +impl From for Timed> { + fn from(value: L3GD20Gyro) -> Self { + Self { + time: value.time, + reading: value.into(), + } + } +} + +impl Deref for Timed { + type Target = R; + + fn deref(&self) -> &Self::Target { + &self.reading + } +} + +fn main() -> Result<(), Box> { + let gyro = + read_csv::(format!("tests/data/{DATASET}/106-gyro-i16-x1.csv").as_str())?; + let compass = read_csv::( + format!("tests/data/{DATASET}/30-mag-i16-x3.csv").as_str(), + )?; + let accel = read_csv::( + format!("tests/data/{DATASET}/25-acc-i16-x3.csv").as_str(), + )?; + + // Obtain the offset times. + let gyro_t = gyro[0].time; + let accel_t = accel[0].time; + let compass_t = compass[0].time; + let time_offset = gyro_t.min(accel_t).min(compass_t); + + // Convert the readings into normalized frames. + let gyro: Vec>> = gyro + .into_iter() + .map(Timed::from) + .map(|t| t.with_time_offset(time_offset)) + .collect(); + let compass: Vec>> = compass + .into_iter() + .map(Timed::from) + .map(|t| t.with_time_offset(time_offset)) + .collect(); + let accel: Vec>> = accel + .into_iter() + .map(Timed::from) + .map(|t| t.with_time_offset(time_offset)) + .collect(); + + // Determine sample rates. + let (gyro_sample_rate, _) = determine_sampling(&gyro); + let (accel_sample_rate, _) = determine_sampling(&accel); + let (compass_sample_rate, _) = determine_sampling(&compass); + + println!("Average sample rates:"); + println!("- Accelerometer readings: {accel_sample_rate} Hz (expected 400 Hz)"); + println!("- Magnetometer readings: {compass_sample_rate} Hz (expected 75 Hz)"); + println!("- Gyroscope readings: {gyro_sample_rate} Hz (expected 400 Hz)"); + + // Magnetic field reference for Berlin, Germany expressed in North, East, Down. + let reference = MagneticReference::new(18.0, 1.5, 47.0); + + // Create the estimator. + let mut estimator = OwnedOrientationEstimator::::new( + AccelerometerNoise::new(0.07, 0.07, 0.07), + MagnetometerNoise::new(0.18, 0.11, 0.34), + reference, + 0.001, + ); + + let mut gyro_x_estimator = + marg_orientation::gyro_drift::GyroscopeAxisEstimator::::new(0.00003, 5.0, 0.001); + + // Prepare some basics for the simulation. + let font = Font::new(Path::new( + "examples/data/fonts/firamono/FiraMono-Regular.ttf", + )) + .expect("failed to load font"); + + let mut window = Window::new("MPU6050 and HMC8533L simulation"); + window.set_framerate_limit(Some(30)); + window.set_light(Light::StickToCamera); + + // Create a box at the coordinate origin. + let mut c = window.add_cube(0.02, 0.02, 0.02); + c.set_color(1.0, 1.0, 1.0); + + // Create the arrow indicating orientation. + let mut arrows = create_arrow(&mut window); + + // Some colors. + let red = Point3::new(1.0, 0.0, 0.0); + let green = Point3::new(0.0, 1.0, 0.0); + let blue = Point3::new(0.0, 0.0, 1.0); + let dark_red = red * 0.5; + let dark_green = green * 0.5; + let dark_blue = blue * 0.5; + + // Walk through the simulation data. + let mut accel_index = 0; + let mut compass_index = 0; + let mut gyro_index = 0; + + let mut last_time = Instant::now(); + let mut simulation_time = Duration::default(); + let mut is_paused = false; + let mut reset_times = false; + let mut display_reference = true; + let mut display_body_frame = false; + let mut display_sensors = true; + + 'render: while window.render() { + // Obtain the current render timestamp. + let now = Instant::now(); + let elapsed_time = now - last_time; + last_time = now; + + if reset_times { + reset_times = false; + simulation_time = Duration::default(); + accel_index = 0; + gyro_index = 0; + compass_index = 0; + } + + // Handle window events to check for key presses + for event in window.events().iter() { + if let WindowEvent::Key(key, Action::Press, _) = event.value { + if key == Key::Space { + is_paused = !is_paused; + } else if key == Key::R { + reset_times = true; + continue; + } else if key == Key::C { + display_reference = !display_reference; + continue; + } else if key == Key::B { + display_body_frame = !display_body_frame; + continue; + } else if key == Key::S { + display_sensors = !display_sensors; + continue; + } + } + } + + if !is_paused { + simulation_time += elapsed_time; + window.set_background_color(0.118, 0.122, 0.149); + } else { + window.set_background_color(0.149, 0.122, 0.118); + } + + // Enable updates when we receive new data. + let mut acc_should_update = false; + let mut mag_should_update = false; + let mut gyro_should_update = false; + + // Increment simulation index. + if !is_paused { + while accel[accel_index].time < simulation_time.as_secs_f64() { + accel_index += 1; + acc_should_update = true; + if accel_index >= accel.len() { + reset_times = true; + continue 'render; + } + } + while gyro[gyro_index].time < simulation_time.as_secs_f64() { + gyro_index += 1; + gyro_should_update = true; + if accel_index >= gyro.len() { + reset_times = true; + continue 'render; + } + } + while compass[compass_index].time < simulation_time.as_secs_f64() { + compass_index += 1; + mag_should_update = true; + if compass_index >= compass.len() { + reset_times = true; + continue 'render; + } + } + } + + let accel_meas = &accel[accel_index]; + let gyro_meas = &gyro[gyro_index]; + let compass_meas = &compass[compass_index]; + + // Calculate the angle between the magnetic field vector and the down vector. + let angle_mag_accel = calculate_angle_acc_mag(accel_meas, compass_meas); + + // Calculated heading. + let heading_degrees = { + let vec = Vector3::new(compass_meas.x, compass_meas.y, compass_meas.z).normalize(); + let heading = vec[1].atan2(vec[0]).to_degrees(); + if heading >= 360.0 { + heading - 360.0 + } else if heading <= 0.0 { + heading + 360.0 + } else { + heading + } + }; + + // Run a prediction. + if !is_paused { + estimator.predict(); + gyro_x_estimator.predict(elapsed_time.as_secs_f32()); + } + + let estimated_angles = estimator.estimated_angles(); + if estimated_angles.yaw_psi.is_nan() + || estimated_angles.pitch_theta.is_nan() + || estimated_angles.roll_phi.is_nan() + { + todo!("nan before correction"); + } + + // Update the filter when needed. + if acc_should_update { + estimator.correct_accelerometer(&accel_meas.reading); + } + if mag_should_update { + estimator.correct_magnetometer(&compass_meas.reading); + } + if gyro_should_update { + gyro_x_estimator.correct(gyro_meas.reading.omega_x); + } + + let estimated_angles = estimator.estimated_angles(); + if estimated_angles.yaw_psi.is_nan() + || estimated_angles.pitch_theta.is_nan() + || estimated_angles.roll_phi.is_nan() + { + todo!("nan after correction"); + } + + // Obtain a rotation matrix from the estimated angles. + let north = estimator.rotate_vector_world(marg_orientation::Vector3::new(1.0, 0.0, 0.0)); + let east = estimator.rotate_vector_world(marg_orientation::Vector3::new(0.0, 1.0, 0.0)); + let down = estimator.rotate_vector_world(marg_orientation::Vector3::new(0.0, 0.0, 1.0)); + let filter_x = kiss3d_point(NorthEastDown::new(north.x, north.y, north.z)); + let filter_y = kiss3d_point(NorthEastDown::new(east.x, east.y, east.z)); + let filter_z = kiss3d_point(NorthEastDown::new(down.x, down.y, down.z)); + + // Update the arrow according to the estimations. + update_arrow_orientation(&mut arrows, filter_x, filter_y, filter_z); + + // Display default coordinate system. + if display_reference { + let x_axis = NorthEastDown::new(1.0, 0.0, 0.0); + let y_axis = NorthEastDown::new(0.0, 1.0, 0.0); + let z_axis = NorthEastDown::new(0.0, 0.0, 1.0); + + window.draw_line(&Point3::default(), &kiss3d_point(x_axis), &dark_red); + window.draw_line(&Point3::default(), &kiss3d_point(y_axis), &dark_green); + window.draw_line(&Point3::default(), &kiss3d_point(z_axis), &dark_blue); + } + + // Convert estimations. + if display_body_frame { + // Display estimated orientation. + window.draw_line(&Point3::default(), &filter_x, &red); + window.draw_line(&Point3::default(), &filter_y, &green); + window.draw_line(&Point3::default(), &filter_z, &blue); + } + + // Display the accelerometer reading. + if display_sensors { + let am = NorthEastDown::new(accel_meas.x, accel_meas.y, accel_meas.z); + let p1 = Point3::new(0.0, 0.0, 0.0); + window.draw_line(&p1, &kiss3d_point(am), &Point3::new(0.5, 0.0, 1.0)); + } + + // Display the compass reading. + if display_sensors { + let mm = NorthEastDown::new(compass_meas.x, compass_meas.y, compass_meas.z); + let p1 = Point3::new(0.0, 0.0, 0.0); + window.draw_line(&p1, &kiss3d_point(mm), &Point3::new(1.0, 0.0, 0.5)); + } + + display_times_and_indexes( + &gyro, + &compass, + &accel, + &font, + &mut window, + accel_index, + compass_index, + gyro_index, + simulation_time, + elapsed_time, + ); + + // Display angle between measured accelerometer and magnetometer. + let info = format!("cos⁻¹(acc·mag) = {:+0.02}°", angle_mag_accel.to_degrees()); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 18.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display angle between measured accelerometer and magnetometer. + let info = format!("heading = {:+0.02}°", heading_degrees); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 17.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display magnetometer. + let info = format!("Bx = {:+0.02} Gs", compass_meas.x); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 15.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display magnetometer. + let info = format!("By = {:+0.02} Gs", compass_meas.y); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 14.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display magnetometer. + let info = format!("Bz = {:+0.02} Gs", compass_meas.z); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 13.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display acceleration. + let info = format!("ax = {:+0.02} G", accel_meas.x); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 11.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display acceleration. + let info = format!("ay = {:+0.02} G", accel_meas.y); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 10.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display acceleration. + let info = format!("az = {:+0.02} G", accel_meas.z); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 9.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display gyro roll rates. + let estimated_omega_x = gyro_x_estimator.angular_velocity(); + let estimated_x_bias = gyro_x_estimator.bias(); + let info = format!( + "ωx = {:+0.02} rad/s ({:+0.02}°/s) - {:+0.02} rad/s ± {:+0.02}rad/s", + gyro_meas.omega_x, + gyro_meas.omega_x * 180.0 / std::f32::consts::PI, + estimated_omega_x, + estimated_x_bias + ); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 7.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display gyro roll rates. + let info = format!( + "ωy = {:+0.02} rad/s ({:+0.02}°/s)", + gyro_meas.omega_y, + gyro_meas.omega_y * 180.0 / std::f32::consts::PI + ); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 6.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display gyro roll rates. + let info = format!( + "ωz = {:+0.02} rad/s ({:+0.02}°/s)", + gyro_meas.omega_z, + gyro_meas.omega_z * 180.0 / std::f32::consts::PI + ); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 5.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display estimated angles. + let info = format!( + "φ = {:+0.02} ± {:+0.02} rad ({:+0.02}°)", + estimated_angles.roll_phi, + estimator.roll_variance().sqrt(), + estimated_angles.roll_phi * 180.0 / std::f32::consts::PI, + ); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 3.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display estimated angles. + let info = format!( + "θ = {:+0.02} ± {:+0.02} rad ({:+0.02}°)", + estimated_angles.pitch_theta, + estimator.pitch_variance().sqrt(), + estimated_angles.pitch_theta * 180.0 / std::f32::consts::PI, + ); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0 * 2.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display estimated angles. + let info = format!( + "ψ = {:+0.02} ± {:+0.02} rad ({:+0.02}°)", + estimated_angles.yaw_psi, + estimator.yaw_variance().sqrt(), + estimated_angles.yaw_psi * 180.0 / std::f32::consts::PI, + ); + window.draw_text( + &info, + &Point2::new(0.0, 1200.0 - 32.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + } + + Ok(()) +} + +fn display_times_and_indexes( + gyro: &[Timed>], + compass: &[Timed>], + accel: &[Timed>], + font: &Rc, + window: &mut Window, + accel_index: usize, + compass_index: usize, + gyro_index: usize, + simulation_time: Duration, + elapsed_time: Duration, +) { + // Display elapsed time since last frame. + let info = format!( + "ΔT = {:.4} s ({:.2}) Hz", + elapsed_time.as_secs_f32(), + elapsed_time.as_secs_f32().recip() + ); + window.draw_text( + &info, + &Point2::new(0.0, 0.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display simulation time. + let info = format!(" t = {:.2} s", simulation_time.as_secs_f32()); + window.draw_text( + &info, + &Point2::new(0.0, 32.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display simulation indexes. + let info = format!("ta = {:.2} s (#{})", accel[accel_index].time, accel_index); + window.draw_text( + &info, + &Point2::new(0.0, 32.0 + 32.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display simulation indexes. + let info = format!( + "tm = {:.2} s (#{})", + compass[compass_index].time, compass_index + ); + window.draw_text( + &info, + &Point2::new(0.0, 32.0 + 2.0 * 32.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); + + // Display simulation indexes. + let info = format!("tg = {:.2} s (#{})", gyro[gyro_index].time, gyro_index); + window.draw_text( + &info, + &Point2::new(0.0, 32.0 + 3.0 * 32.0), + 32.0, + &font, + &Point3::new(1.0, 1.0, 1.0), + ); +} + +fn create_arrow(window: &mut Window) -> SceneNode { + // Define the vertices for the arrow, based on a rectangle (shaft) and triangles (head). + let arrow_vertices = vec![ + // Rectangle vertices (shaft) on x-z plane, half as long + Point3::new(-0.20, 0.0, -0.05), // Bottom-left + Point3::new(0.10, 0.0, -0.05), // Bottom-right + Point3::new(0.10, 0.0, 0.05), // Top-right + Point3::new(-0.20, 0.0, 0.05), // Top-left + // Triangle vertices (head) on x-z plane, half as long + Point3::new(0.10, 0.0, -0.1), // Bottom + Point3::new(0.30, 0.0, 0.0), // Tip + Point3::new(0.10, 0.0, 0.1), // Top + ]; + + // Define the indices for the arrow. + let arrow_indices = vec![ + // Rectangle (shaft) + Point3::new(0u16, 1, 2), + Point3::new(0, 2, 3), + // Triangle (head) + Point3::new(4, 5, 6), + ]; + + // Create the mesh from vertices and indices + let arrow_mesh = Mesh::new(arrow_vertices, arrow_indices, None, None, false); + let arrow_mesh = Rc::new(RefCell::new(arrow_mesh)); + + // Quaternion to flip the arrow around once. + let flip_quaternion = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), std::f32::consts::PI); + + // Add the mesh to the window + let mut arrows = window.add_group(); + + // The top-side of the arrow (red). + let mut top_arrow = arrows.add_mesh(arrow_mesh.clone(), Vector3::new(1.0, 1.0, 1.0)); + top_arrow.set_color(0.8, 0.165, 0.212); + top_arrow.set_local_translation(Translation3::new(0.0, -0.005, 0.0)); + + // Flipped version to work around backface culling. + let mut top_arrow = arrows.add_mesh(arrow_mesh.clone(), Vector3::new(1.0, 1.0, 1.0)); + top_arrow.set_color(0.545, 0.114, 0.145); + top_arrow.set_local_translation(Translation3::new(0.0, -0.005, 0.0)); + top_arrow.set_local_rotation(flip_quaternion); + + // The bottom-side of the arrow (white). + let mut bottom_arrow = arrows.add_mesh(arrow_mesh.clone(), Vector3::new(1.0, 1.0, 1.0)); + bottom_arrow.set_color(1.0, 1.0, 1.0); + bottom_arrow.set_local_translation(Translation3::new(0.0, 0.005, 0.0)); + + // Flipped version to work around backface culling. + let mut bottom_arrow = arrows.add_mesh(arrow_mesh, Vector3::new(1.0, 1.0, 1.0)); + bottom_arrow.set_color(1.0, 1.0, 1.0); + bottom_arrow.set_local_translation(Translation3::new(0.0, 0.005, 0.0)); + bottom_arrow.set_local_rotation(flip_quaternion); + arrows +} + +fn calculate_angle_acc_mag( + accel_meas: &Timed>, + compass_meas: &Timed>, +) -> f32 { + let accel_vec: Vector3<_> = Vector3::new(accel_meas.x, accel_meas.y, accel_meas.z).normalize(); + let mag_vec: Vector3<_> = + Vector3::new(compass_meas.x, compass_meas.y, compass_meas.z).normalize(); + accel_vec.dot(&mag_vec).acos() +} + +fn determine_sampling(data: &[T]) -> (f64, f64) { + let (total_diff, count) = data.windows(2).fold((0.0, 0), |(sum, cnt), window| { + let diff = window[1].time() - window[0].time(); + (sum + diff, cnt + 1) + }); + let sample_time = total_diff / (count as f64); + let sample_rate = 1.0 / sample_time; + (sample_rate, sample_time) +} + +fn read_csv(file_path: &str) -> Result, Box> { + let mut rdr = ReaderBuilder::new().from_path(file_path)?; + let mut data = Vec::new(); + + for result in rdr.deserialize() { + let record: T = result?; + data.push(record); + } + + Ok(data) +} + +fn kiss3d_point(vector: C) -> Point3 +where + C: Into>, + T: Scalar, +{ + let vector = vector.into(); + Point3::new(vector.x(), vector.y(), vector.z()) +} + +// Function to update the arrow's orientation based on new basis vectors +fn update_arrow_orientation( + arrow: &mut SceneNode, + x_basis: Point3, + y_basis: Point3, + z_basis: Point3, +) { + let x_basis = Vector3::new(x_basis[0], x_basis[1], x_basis[2]); + let y_basis = Vector3::new(y_basis[0], y_basis[1], y_basis[2]); + let z_basis = Vector3::new(z_basis[0], z_basis[1], z_basis[2]); + + // Create a rotation matrix from the orthonormal basis vectors + let rotation_matrix = Matrix3::from_columns(&[x_basis, y_basis, z_basis]); + + // Convert the rotation matrix to a UnitQuaternion + let rotation = UnitQuaternion::from_matrix(&rotation_matrix); + let flip = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), std::f32::consts::FRAC_PI_2); + + let rotation = rotation * flip; + + // Apply the rotation to the arrow + arrow.set_local_rotation(rotation); +} diff --git a/math/quaternion_jacobian.m b/math/quaternion_jacobian.m new file mode 100644 index 0000000..159ab9d --- /dev/null +++ b/math/quaternion_jacobian.m @@ -0,0 +1,42 @@ +% Define symbolic variables +syms q0 q1 q2 q3 real % quaternion components +syms x y z real % components of the 3D vector + +% Define the quaternion +q = [q0 q1 q2 q3]; + +% Define the 3D vector +v = [x; y; z]; + +% Down vector. +% v(1) = 0; +% v(2) = 0; +% v(3) = 1; + +% Quaternion multiplication (q*v*q') +q_conj = [q0, -q1, -q2, -q3]; % Conjugate of the quaternion + +% Quaternion-vector multiplication q*v +v_quat = [0; v]; % Extend vector to quaternion form +qv = quat_mult(q, v_quat); % Quaternion multiplication q*v + +% Quaternion-vector multiplication (q*v)*q' +v_rot_quat = quat_mult(qv, q_conj); + +% Extract rotated vector +v_rot = v_rot_quat(2:4); + +% Compute the Jacobian of the rotated vector with respect to the quaternion +J = jacobian(v_rot, q); + +% Display the Jacobian +disp('Jacobian of the rotated vector with respect to the quaternion:'); +disp(J); + +function res = quat_mult(q1, q2) + % Quaternion multiplication: q1 * q2 + res = [q1(1)*q2(1) - q1(2)*q2(2) - q1(3)*q2(3) - q1(4)*q2(4); + q1(1)*q2(2) + q1(2)*q2(1) + q1(3)*q2(4) - q1(4)*q2(3); + q1(1)*q2(3) - q1(2)*q2(4) + q1(3)*q2(1) + q1(4)*q2(2); + q1(1)*q2(4) + q1(2)*q2(3) - q1(3)*q2(2) + q1(4)*q2(1)]; +end diff --git a/src/accelerometer_noise.rs b/src/accelerometer_noise.rs index 2ff5c25..5b3ba77 100644 --- a/src/accelerometer_noise.rs +++ b/src/accelerometer_noise.rs @@ -1,7 +1,9 @@ use crate::impl_standard_traits; use core::fmt::{Debug, Formatter}; use core::ops::Mul; +use uniform_array_derive::UniformArray; +#[derive(UniformArray)] #[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] #[repr(C)] pub struct AccelerometerNoise { @@ -19,12 +21,6 @@ impl AccelerometerNoise { pub const fn new(x: T, y: T, z: T) -> Self { Self { x, y, z } } - - /// Returns the length of the [`AccelerometerNoise`] vector. - #[inline(always)] - pub const fn len(&self) -> usize { - 3 - } } impl Default for AccelerometerNoise @@ -78,34 +74,6 @@ where } } -#[cfg(not(feature = "unsafe"))] -impl core::ops::Index for AccelerometerNoise { - type Output = T; - - #[inline(always)] - fn index(&self, index: usize) -> &Self::Output { - match index { - 0 => &self.x, - 1 => &self.y, - 2 => &self.z, - _ => panic!("Index out of bounds"), - } - } -} - -#[cfg(not(feature = "unsafe"))] -impl core::ops::IndexMut for AccelerometerNoise { - #[inline(always)] - fn index_mut(&mut self, index: usize) -> &mut Self::Output { - match index { - 0 => &mut self.x, - 1 => &mut self.y, - 2 => &mut self.z, - _ => panic!("Index out of bounds"), - } - } -} - impl_standard_traits!(AccelerometerNoise, T); #[cfg(test)] diff --git a/src/accelerometer_reading.rs b/src/accelerometer_reading.rs index f18ead1..5da998a 100644 --- a/src/accelerometer_reading.rs +++ b/src/accelerometer_reading.rs @@ -1,7 +1,9 @@ use crate::impl_standard_traits; use core::fmt::{Debug, Formatter}; use core::ops::Mul; +use uniform_array_derive::UniformArray; +#[derive(UniformArray)] #[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] #[repr(C)] pub struct AccelerometerReading { @@ -20,10 +22,20 @@ impl AccelerometerReading { Self { x, y, z } } - /// Returns the length of the [`AccelerometerReading`] vector. - #[inline(always)] - pub const fn len(&self) -> usize { - 3 + /// Constructs a new [`AccelerometerReading`] instance from a reading in a given coordinate frame. + #[cfg(feature = "coordinate-frame")] + #[cfg_attr(docsrs, doc(cfg(feature = "coordinate-frame")))] + pub fn north_east_down(coordinate: C) -> Self + where + C: Into>, + T: Clone, + { + let coordinate = coordinate.into(); + Self { + x: coordinate.x(), + y: coordinate.y(), + z: coordinate.z(), + } } } @@ -78,31 +90,15 @@ where } } -#[cfg(not(feature = "unsafe"))] -impl core::ops::Index for AccelerometerReading { - type Output = T; - - #[inline(always)] - fn index(&self, index: usize) -> &Self::Output { - match index { - 0 => &self.x, - 1 => &self.y, - 2 => &self.z, - _ => panic!("Index out of bounds"), - } - } -} - -#[cfg(not(feature = "unsafe"))] -impl core::ops::IndexMut for AccelerometerReading { - #[inline(always)] - fn index_mut(&mut self, index: usize) -> &mut Self::Output { - match index { - 0 => &mut self.x, - 1 => &mut self.y, - 2 => &mut self.z, - _ => panic!("Index out of bounds"), - } +#[cfg(feature = "coordinate-frame")] +#[cfg_attr(docsrs, doc(cfg(feature = "coordinate-frame")))] +impl From for AccelerometerReading +where + C: coordinate_frame::CoordinateFrame, + T: Copy + coordinate_frame::SaturatingNeg, +{ + fn from(value: C) -> Self { + Self::north_east_down(value.to_ned()) } } diff --git a/src/euler_angles.rs b/src/euler_angles.rs index 7f08902..b68cf6b 100644 --- a/src/euler_angles.rs +++ b/src/euler_angles.rs @@ -1,6 +1,8 @@ use crate::impl_standard_traits; use core::fmt::{Debug, Formatter}; +use uniform_array_derive::UniformArray; +#[derive(UniformArray)] #[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] #[repr(C)] pub struct EulerAngles { @@ -22,13 +24,6 @@ impl EulerAngles { yaw_psi, } } - - /// Returns the length of the [`EulerAngles`] vector. - #[inline(always)] - #[allow(unused)] - pub const fn len(&self) -> usize { - 3 - } } impl Default for EulerAngles @@ -67,36 +62,6 @@ where } } -#[cfg_attr(docsrs, doc(cfg(not(feature = "unsafe"))))] -#[cfg(not(feature = "unsafe"))] -impl core::ops::Index for EulerAngles { - type Output = T; - - #[inline(always)] - fn index(&self, index: usize) -> &Self::Output { - match index { - 0 => &self.roll_phi, - 1 => &self.pitch_theta, - 2 => &self.yaw_psi, - _ => panic!("Index out of bounds"), - } - } -} - -#[cfg_attr(docsrs, doc(cfg(not(feature = "unsafe"))))] -#[cfg(not(feature = "unsafe"))] -impl core::ops::IndexMut for EulerAngles { - #[inline(always)] - fn index_mut(&mut self, index: usize) -> &mut Self::Output { - match index { - 0 => &mut self.roll_phi, - 1 => &mut self.pitch_theta, - 2 => &mut self.yaw_psi, - _ => panic!("Index out of bounds"), - } - } -} - impl_standard_traits!(EulerAngles, T); #[cfg(test)] diff --git a/src/gyro_drift.rs b/src/gyro_drift.rs new file mode 100644 index 0000000..03d6a17 --- /dev/null +++ b/src/gyro_drift.rs @@ -0,0 +1,6 @@ +//! Estimations for gyroscope measurements. + +mod filter; +mod types; + +pub use filter::*; diff --git a/src/gyro_drift/filter.rs b/src/gyro_drift/filter.rs new file mode 100644 index 0000000..7150064 --- /dev/null +++ b/src/gyro_drift/filter.rs @@ -0,0 +1,290 @@ +use minikalman::buffers::types::*; +use minikalman::matrix::MatrixDataType; +use minikalman::prelude::*; +use minikalman::regular::{RegularKalmanBuilder, RegularObservationBuilder}; + +use crate::gyro_drift::types::*; + +/// An estimator for gyroscope axis-specific angular velocity and bias. +pub struct GyroscopeAxisEstimator { + filter: OwnedKalmanFilter, + measurement: OwnedObservation, +} + +impl GyroscopeAxisEstimator { + /// Initializes a new instance of the [`GyroscopeAxisEstimator`] struct. + /// + /// ## Arguments + /// * `drift_estimate` - The initial estimate for the axis drift value. + /// * `gyro_noise` - The gyroscope noise values (sigma-squared) for the axis. + /// * `process_noise` - A process noise value. + pub fn new(drift_estimate: T, gyro_noise: T, process_noise: T) -> Self + where + T: MatrixDataType + Default, + { + let filter = Self::build_filter(drift_estimate, gyro_noise, process_noise); + let measurement = Self::build_measurement(gyro_noise); + + Self { + filter, + measurement, + } + } +} + +impl GyroscopeAxisEstimator { + /// Performs a prediction step to obtain new gyroscope estimates. + pub fn predict(&mut self, delta_t: T) + where + T: MatrixDataType, + { + self.filter.state_transition_mut().set(0, 1, delta_t); + self.filter.predict(); + } + + /// Performs a correction step using accelerometer and magnetometer readings. + /// + /// ## Arguments + /// * `accelerometer` - The accelerometer reading. + pub fn correct(&mut self, angular_velocity: T) + where + T: MatrixDataType, + { + self.measurement.measurement_vector_mut().apply(|vec| { + vec.set_row(0, -angular_velocity); + }); + + // Perform the update step. + self.filter.correct(&mut self.measurement); + } + + /// Gets the estimated angular velocity. + pub fn angular_velocity(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(0) + } + + /// Gets the estimated bias term. + pub fn bias(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(1) + } +} + +impl GyroscopeAxisEstimator { + /// Builds the Kalman filter used for prediction. + fn build_filter( + drift_estimate: T, + sensor_noise: T, + process_noise_value: T, + ) -> OwnedKalmanFilter + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // State vector. + let mut state_vec = + StateVectorBuffer::::new(MatrixData::new_array::( + [zero; STATES], + )); + state_vec.apply(|vec| { + vec.set_row(0, T::zero()); + vec.set_row(1, drift_estimate); + }); + + // State transition matrix. + let mut state_transition = + StateTransitionMatrixMutBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + state_transition.make_identity(); + + // Estimate covariance matrix. + let mut estimate_covariance = + EstimateCovarianceMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + estimate_covariance.make_scalar(sensor_noise); + + // Process noise matrix. + let mut process_noise = DirectProcessNoiseCovarianceMatrixMutBuffer::::new( + MatrixData::new_array::( + [zero; { STATES * STATES }], + ), + ); + process_noise.make_scalar(process_noise_value); + + // Predicted state vector. + let predicted_state = + PredictedStateEstimateVectorBuffer::::new(MatrixData::new_array::< + STATES, + 1, + STATES, + T, + >([zero; STATES])); + + // Temporary estimate covariance matrix. + let temp_state_matrix = + TemporaryStateMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + RegularKalmanBuilder::new::( + state_transition, + state_vec, + estimate_covariance, + process_noise, + predicted_state, + temp_state_matrix, + ) + } + + /// Builds the Kalman filter observation used for the prediction. + fn build_measurement(axis_noise: T) -> OwnedObservation + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // Measurement vector + let measurement = + MeasurementVectorBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + 1, + OBSERVATIONS, + T, + >([zero; OBSERVATIONS])); + + // Observation matrix + let mut observation_matrix = + ObservationMatrixMutBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + STATES, + { OBSERVATIONS * STATES }, + T, + >( + [zero; { OBSERVATIONS * STATES }], + )); + observation_matrix.apply(|mat| { + mat.set_col(0, T::one()); + mat.set_col(1, T::zero()); + }); + + // Measurement noise covariance + let mut noise_covariance = + MeasurementNoiseCovarianceMatrixBuffer::::new( + MatrixData::new_array::< + OBSERVATIONS, + OBSERVATIONS, + { OBSERVATIONS * OBSERVATIONS }, + T, + >([zero; { OBSERVATIONS * OBSERVATIONS }]), + ); + noise_covariance.apply(|mat| { + mat.set_at(0, 0, axis_noise); + }); + + // Innovation vector + let innovation_vector = + InnovationVectorBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + 1, + OBSERVATIONS, + T, + >([zero; OBSERVATIONS])); + + // Innovation covariance matrix + let innovation_covariance = + InnovationCovarianceMatrixBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + OBSERVATIONS, + { OBSERVATIONS * OBSERVATIONS }, + T, + >( + [zero; { OBSERVATIONS * OBSERVATIONS }], + )); + + // Kalman Gain matrix + let kalman_gain = + KalmanGainMatrixBuffer::::new(MatrixData::new_array::< + STATES, + OBSERVATIONS, + { STATES * OBSERVATIONS }, + T, + >( + [zero; { STATES * OBSERVATIONS }], + )); + + // Temporary residual covariance inverted matrix + let temp_sinv = TemporaryResidualCovarianceInvertedMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { OBSERVATIONS * OBSERVATIONS }], + ), + ); + + // Temporary H×P matrix + let temp_hp = + TemporaryHPMatrixBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + STATES, + { OBSERVATIONS * STATES }, + T, + >( + [zero; { OBSERVATIONS * STATES }], + )); + + // Temporary P×Hᵀ matrix + let temp_pht = + TemporaryPHTMatrixBuffer::::new(MatrixData::new_array::< + STATES, + OBSERVATIONS, + { STATES * OBSERVATIONS }, + T, + >( + [zero; { STATES * OBSERVATIONS }], + )); + + // Temporary K×(H×P) matrix + let temp_khp = TemporaryKHPMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + RegularObservationBuilder::new::( + observation_matrix, + measurement, + noise_covariance, + innovation_vector, + innovation_covariance, + kalman_gain, + temp_sinv, + temp_hp, + temp_pht, + temp_khp, + ) + } +} diff --git a/src/gyro_drift/types.rs b/src/gyro_drift/types.rs new file mode 100644 index 0000000..5a30bc9 --- /dev/null +++ b/src/gyro_drift/types.rs @@ -0,0 +1,79 @@ +use minikalman::buffers::types::*; +use minikalman::prelude::*; +use minikalman::regular::{RegularKalman, RegularObservation}; + +pub const STATES: usize = 2; // angular velocity, bias +pub const OBSERVATIONS: usize = 1; // angular velocity + +// A Kalman filter of four states, using owned buffers. +pub type OwnedKalmanFilter = RegularKalman< + STATES, + T, + StateTransitionMatrixMutBuffer< + STATES, + T, + MatrixDataArray, + >, + StateVectorBuffer>, + EstimateCovarianceMatrixBuffer< + STATES, + T, + MatrixDataArray, + >, + DirectProcessNoiseCovarianceMatrixMutBuffer< + STATES, + T, + MatrixDataArray, + >, + PredictedStateEstimateVectorBuffer>, + TemporaryStateMatrixBuffer>, +>; + +/// On observation of three states, using owned buffers. +pub type OwnedObservation = RegularObservation< + STATES, + OBSERVATIONS, + T, + ObservationMatrixMutBuffer< + OBSERVATIONS, + STATES, + T, + MatrixDataArray, + >, + MeasurementVectorBuffer>, + MeasurementNoiseCovarianceMatrixBuffer< + OBSERVATIONS, + T, + MatrixDataArray, + >, + InnovationVectorBuffer>, + InnovationCovarianceMatrixBuffer< + OBSERVATIONS, + T, + MatrixDataArray, + >, + KalmanGainMatrixBuffer< + STATES, + OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryResidualCovarianceInvertedMatrixBuffer< + OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryHPMatrixBuffer< + OBSERVATIONS, + STATES, + T, + MatrixDataArray, + >, + TemporaryPHTMatrixBuffer< + STATES, + OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryKHPMatrixBuffer>, +>; diff --git a/src/gyro_free.rs b/src/gyro_free.rs new file mode 100644 index 0000000..756a1a0 --- /dev/null +++ b/src/gyro_free.rs @@ -0,0 +1,4 @@ +mod filter; +mod types; + +pub use crate::gyro_free::filter::*; diff --git a/src/gyro_free/filter.rs b/src/gyro_free/filter.rs new file mode 100644 index 0000000..a351504 --- /dev/null +++ b/src/gyro_free/filter.rs @@ -0,0 +1,752 @@ +use crate::gyro_free::types::*; +use crate::vector3::Vector3; +use crate::{ + Abs, AccelerometerNoise, AccelerometerReading, ArcSin, ArcTan, EulerAngles, IsNaN, + MagnetometerNoise, MagnetometerReading, NormalizeAngle, +}; +use minikalman::buffers::types::*; +use minikalman::extended::{ExtendedKalmanBuilder, ExtendedObservationBuilder}; +use minikalman::matrix::MatrixDataType; +use minikalman::prelude::*; +use num_traits::Zero; + +/// A magnetic field reference vector. +pub type MagneticReference = MagnetometerReading; + +/// A MARG (Magnetic, Angular Rate, and Gravity) orientation estimator with generic type `T`. +pub struct OwnedOrientationEstimator { + filter: OwnedKalmanFilter, + /// Magnetometer measurements. + mag_measurement: OwnedVector3Observation, + /// Accelerometer measurements. + acc_measurement: OwnedVector3Observation, + /// Magnetic field reference vector for the current location. + magnetic_field_ref: Vector3, +} + +impl OwnedOrientationEstimator { + /// Initializes a new instance of the [`OwnedOrientationEstimator`] struct. + /// + /// ## Arguments + /// * `accelerometer_noise` - The accelerometer noise values (sigma-squared) for each axis. + /// * `magnetometer_noise` - The magnetometer noise values (sigma-squared) for each axis. + /// * `magnetic_field_ref` - The magnetic field reference vector for the current location. + /// * `process_noise` - A process noise value. + /// * `epsilon` - A small bias term to avoid divisions by zero. Set to e.g. `1e-6`. + pub fn new( + accelerometer_noise: AccelerometerNoise, + magnetometer_noise: MagnetometerNoise, + magnetic_field_ref: MagneticReference, + process_noise: T, + ) -> Self + where + T: MatrixDataType + Default, + { + let filter = Self::build_filter(process_noise); + let mag_measurement = Self::build_mag_measurement(&magnetometer_noise); + let acc_measurement = Self::build_accel_measurement(&accelerometer_noise); + + let magnetic_field_ref = Vector3::new( + magnetic_field_ref.x, + magnetic_field_ref.y, + magnetic_field_ref.z, + ) + .normalized(); + + Self { + filter, + mag_measurement, + acc_measurement, + magnetic_field_ref, + } + } +} + +impl OwnedOrientationEstimator { + /// Performs a prediction step to obtain new orientation estimates. + pub fn predict(&mut self) + where + T: MatrixDataType + IsNaN, + { + // Perform a regular Kalman Filter prediction step. + self.filter + .predict_nonlinear(|current, next| current.copy(next)); + self.panic_if_nan(); + } + + fn apply_normalized_vector(vec: V, measurement: &mut M) + where + M: RowVectorMut<3, T>, + V: Into>, + T: MatrixDataType, + { + let vec: Vector3 = vec.into().normalized(); + measurement.set_row(0, vec.x); + measurement.set_row(1, vec.y); + measurement.set_row(2, vec.z); + } + + /// Performs a correction step using accelerometer and magnetometer readings. + /// + /// ## Arguments + /// * `accelerometer` - The accelerometer reading. + pub fn correct_accelerometer(&mut self, accelerometer: &AccelerometerReading) + where + T: MatrixDataType + IsNaN, + { + // Normalize the vectors. + self.acc_measurement.measurement_vector_mut().apply(|vec| { + Self::apply_normalized_vector(accelerometer, vec); + }); + + // Update the Jacobian. + let two = T::one() + T::one(); + let (q0, q1, q2, q3) = self.estimated_quaternion(); + + self.acc_measurement + .observation_jacobian_matrix_mut() + .apply(|mat| { + let two_q0 = q0 * two; + let two_q1 = q1 * two; + let two_q2 = q2 * two; + let two_q3 = q3 * two; + + mat.set_at(0, 0, two_q2); + mat.set_at(0, 1, two_q3); + mat.set_at(0, 2, two_q0); + mat.set_at(0, 3, two_q1); + + mat.set_at(1, 0, -two_q1); + mat.set_at(1, 1, -two_q0); + mat.set_at(1, 2, two_q3); + mat.set_at(1, 3, two_q2); + + mat.set_at(2, 0, two_q0); + mat.set_at(2, 1, -two_q1); + mat.set_at(2, 2, -two_q2); + mat.set_at(2, 3, two_q3); + }); + + // Perform the update step. + self.filter + .correct_nonlinear(&mut self.acc_measurement, |state, measurement| { + let down = Vector3::new(T::zero(), T::zero(), T::one()); + let rotated = Self::rotate_vector_internal(state, down); + measurement.set_row(0, rotated.x); + measurement.set_row(1, rotated.y); + measurement.set_row(2, rotated.z); + + measurement.set_row(0, two * (q1 * q3 + q0 * q2)); + measurement.set_row(1, two * (q2 * q3 - q0 * q1)); + measurement.set_row(2, T::one() - two * (q1 * q1 + q2 * q2)); + }); + + self.normalize_state_quaternion(); + self.panic_if_nan(); + } + + /// Performs a correction step using accelerometer readings. + /// + /// ## Arguments + /// * `magnetometer` - The magnetometer reading. + pub fn correct_magnetometer(&mut self, magnetometer: &MagnetometerReading) + where + T: MatrixDataType + IsNaN, + { + // Normalize the vector. + self.mag_measurement.measurement_vector_mut().apply(|vec| { + Self::apply_normalized_vector(magnetometer, vec); + }); + + // Update the Jacobian. + let one = T::one(); + let two = one + one; + let (q0, q1, q2, q3) = self.estimated_quaternion(); + let (mx, my, mz) = self.magnetic_field_ref.into(); + self.mag_measurement + .observation_jacobian_matrix_mut() + .apply(|mat| { + mat.set_at(0, 0, two * (q0 * mx - q3 * my + q2 * mz)); + mat.set_at(0, 1, two * (q1 * mx + q2 * my + q3 * mz)); + mat.set_at(0, 2, two * (q1 * my - q2 * mx + q0 * mz)); + mat.set_at(0, 3, two * (q1 * mz - q0 * my - q3 * mx)); + + mat.set_at(1, 0, two * (q3 * mx + q0 * my - q1 * mz)); + mat.set_at(1, 1, two * (q2 * mx - q1 * my - q0 * mz)); + mat.set_at(1, 2, two * (q1 * mx + q2 * my + q3 * mz)); + mat.set_at(1, 3, two * (q0 * mx - q3 * my + q2 * mz)); + + mat.set_at(2, 0, two * (q1 * my - q2 * mx + q0 * mz)); + mat.set_at(2, 1, two * (q3 * mx + q0 * my - q1 * mz)); + mat.set_at(2, 2, two * (q3 * my - q0 * mx - q2 * mz)); + mat.set_at(2, 3, two * (q1 * mx + q2 * my + q3 * mz)); + }); + + // Perform the update step. + self.filter + .correct_nonlinear(&mut self.mag_measurement, |state, measurement| { + let reference = self.magnetic_field_ref; + let rotated = Self::rotate_vector_internal(state, reference); + measurement.set_row(0, rotated.x); + measurement.set_row(1, rotated.y); + measurement.set_row(2, rotated.z); + }); + + self.normalize_state_quaternion(); + self.panic_if_nan(); + } + + fn rotate_vector_internal(state: &V, vec: Vector3) -> Vector3 + where + V: RowVectorMut, + T: MatrixDataType, + { + let q0 = state.get_row(0); + let q1 = state.get_row(1); + let q2 = state.get_row(2); + let q3 = state.get_row(3); + + let one = T::one(); + let two = one + one; + let x = vec.x * (one - two * (q2 * q2 + q3 * q3)) + + vec.y * two * (q1 * q2 - q0 * q3) + + vec.z * two * (q1 * q3 + q0 * q2); + + let y = vec.x * two * (q1 * q2 + q0 * q3) + + vec.y * (one - two * (q1 * q1 + q3 * q3)) + + vec.z * two * (q2 * q3 - q0 * q1); + + let z = vec.x * two * (q1 * q3 - q0 * q2) + + vec.y * two * (q2 * q3 + q0 * q1) + + vec.z * (one - two * (q1 * q1 + q2 * q2)); + + Vector3::new(x, y, z) + } + + /// Rotate a vector in the body frame. + /// + /// For display purposes you likely want to use [`rotate_vector_world`](Self::rotate_vector_world) instead. + pub fn rotate_vector_body(&self, vector: Vector3) -> Vector3 + where + T: MatrixDataType, + { + Self::rotate_vector_internal(self.filter.state_vector(), vector) + } + + /// Rotates a vector in the world frame. + pub fn rotate_vector_world(&self, vec: Vector3) -> Vector3 + where + T: MatrixDataType, + { + let state = self.filter.state_vector(); + let q0 = state.get_row(0); + let q1 = state.get_row(1); + let q2 = state.get_row(2); + let q3 = state.get_row(3); + + let one = T::one(); + let two = one + one; + let x = vec.x * (one - two * (q2 * q2 + q3 * q3)) + + vec.y * two * (q1 * q2 + q0 * q3) + + vec.z * two * (q1 * q3 - q0 * q2); + + let y = vec.x * two * (q1 * q2 - q0 * q3) + + vec.y * (one - two * (q1 * q1 + q3 * q3)) + + vec.z * two * (q2 * q3 + q0 * q1); + + let z = vec.x * two * (q1 * q3 + q0 * q2) + + vec.y * two * (q2 * q3 - q0 * q1) + + vec.z * (one - two * (q1 * q1 + q2 * q2)); + + Vector3::new(x, y, z) + } + + fn normalize_state_quaternion(&mut self) + where + T: MatrixDataType, + { + self.filter.state_vector_mut().apply(|vec| { + let w = vec.get_row(0); + let x = vec.get_row(1); + let y = vec.get_row(2); + let z = vec.get_row(3); + + let (w, x, y, z) = if w >= T::zero() { + (w, x, y, z) + } else { + (-w, -x, -y, -z) + }; + + let norm_sq = w * w + x * x + y * y + z * z; + let norm = norm_sq.square_root(); + let w = w / norm; + let x = x / norm; + let y = y / norm; + let z = z / norm; + vec.set_row(0, w); + vec.set_row(1, x); + vec.set_row(2, y); + vec.set_row(3, z); + }); + } + + #[allow(unused)] + fn panic_if_nan(&self) + where + T: Copy + IsNaN, + { + #[cfg(debug_assertions)] + self.filter.state_vector().inspect(|vec| { + if vec.get_row(0).is_nan() || vec.get_row(1).is_nan() || vec.get_row(2).is_nan() { + panic!("NaN angle detected in state estimate") + } + }); + } + + /// Gets the estimated quaternion in (w, x, y, z) order. + fn estimated_quaternion(&self) -> (T, T, T, T) + where + T: Copy, + { + self.filter.state_vector().inspect(|vec| { + ( + vec.get_row(0), + vec.get_row(1), + vec.get_row(2), + vec.get_row(3), + ) + }) + } + + /// Obtains the current estimates of the roll, pitch and yaw angles. + pub fn estimated_angles(&self) -> EulerAngles + where + T: MatrixDataType + + Abs + + ArcSin + + ArcTan + + NormalizeAngle, + { + EulerAngles::new(self.roll(), self.pitch(), self.yaw()) + } + + /// Obtains the current estimate of the roll angle φ (phi), in radians. + /// + /// The roll angle is defined as the amount rotation around the y-axis (forward). + pub fn roll(&self) -> T + where + T: MatrixDataType + ArcTan + NormalizeAngle, + { + let (w, x, y, z) = self.estimated_quaternion(); + let one = T::one(); + let two = one + one; + let sinr_cosp = two * (w * x + y * z); + let cosr_cosp = one - two * (x * x + y * y); + let roll = sinr_cosp.atan2(cosr_cosp); + roll.normalize_angle() + } + + /// Obtains the current estimation variance (uncertainty) of the roll angle φ (phi), in radians². + /// + /// ## Interpretation + /// - Low Variance: Indicates high certainty in the estimate. The state estimate is + /// considered to be precise, as it doesn't vary much from the mean. + /// - High Variance: Indicates high uncertainty in the estimate. The state estimate is + /// considered to be less precise, as it has a wide spread around the mean. + pub fn roll_variance(&self) -> T + where + T: Zero, + { + T::zero() + } + + /// Obtains the current estimate of the pitch angle θ (theta), in radians. + /// + /// The pitch angle is defined as the amount rotation around the x-axis (right). + pub fn pitch(&self) -> T + where + T: MatrixDataType + Abs + ArcSin + NormalizeAngle, + { + let (w, x, y, z) = self.estimated_quaternion(); + let one = T::one(); + let two = one + one; + let sinp = two * (w * y - z * x); + // TODO: If sin >= 1.0 || sin <= -1.0, clamp to +/- pi/2 instead + let pitch = sinp.arcsin(); + pitch.normalize_angle() + } + + /// Obtains the current estimation variance (uncertainty) of the pitch angle θ (theta), in radians². + /// + /// ## Interpretation + /// - Low Variance: Indicates high certainty in the estimate. The state estimate is + /// considered to be precise, as it doesn't vary much from the mean. + /// - High Variance: Indicates high uncertainty in the estimate. The state estimate is + /// considered to be less precise, as it has a wide spread around the mean. + pub fn pitch_variance(&self) -> T + where + T: Zero, + { + T::zero() + } + + /// Obtains the current estimate of the yaw angle ψ (psi), in radians. + /// + /// The yaw angle is defined as the amount rotation around the z-axis (up). + pub fn yaw(&self) -> T + where + T: MatrixDataType + ArcTan + NormalizeAngle, + { + let (w, x, y, z) = self.estimated_quaternion(); + let one = T::one(); + let two = one + one; + let siny_cosp = two * (w * z + x * y); + let cosy_cosp = one - two * (y * y + z * z); + let yaw = siny_cosp.atan2(cosy_cosp); + yaw.normalize_angle() + } + + /// Obtains the current estimation variance (uncertainty) of the yaw angle ψ (psi), in radians². + /// + /// ## Interpretation + /// - Low Variance: Indicates high certainty in the estimate. The state estimate is + /// considered to be precise, as it doesn't vary much from the mean. + /// - High Variance: Indicates high uncertainty in the estimate. The state estimate is + /// considered to be less precise, as it has a wide spread around the mean. + pub fn yaw_variance(&self) -> T + where + T: Zero, + { + T::zero() + } +} + +impl OwnedOrientationEstimator { + /// Builds the Kalman filter used for prediction. + fn build_filter(process_noise_value: T) -> OwnedKalmanFilter + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // State vector. + let mut state_vec = + StateVectorBuffer::::new(MatrixData::new_array::( + [zero; STATES], + )); + + // TODO: Seed with roll/pitch from accelerometer, yaw (heading) from magnetometer. + state_vec.set_row(0, T::one()); + state_vec.set_row(1, T::zero()); + state_vec.set_row(2, T::zero()); + state_vec.set_row(3, T::zero()); + + // State transition matrix. + let mut state_transition = + StateTransitionMatrixMutBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + state_transition.make_identity(); + + // Estimate covariance matrix. + let mut estimate_covariance = + EstimateCovarianceMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + estimate_covariance.make_scalar(process_noise_value); + + // Process noise matrix. + let mut process_noise = DirectProcessNoiseCovarianceMatrixMutBuffer::::new( + MatrixData::new_array::( + [zero; { STATES * STATES }], + ), + ); + process_noise.make_scalar(process_noise_value); + + // Predicted state vector. + let mut predicted_state = + PredictedStateEstimateVectorBuffer::::new(MatrixData::new_array::< + STATES, + 1, + STATES, + T, + >([zero; STATES])); + predicted_state.set_all(T::zero()); + + // Temporary estimate covariance matrix. + let temp_state_matrix = + TemporaryStateMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + ExtendedKalmanBuilder::new::( + state_transition, + state_vec, + estimate_covariance, + process_noise, + predicted_state, + temp_state_matrix, + ) + } + + /// Builds the Kalman filter observation used for the prediction. + fn build_mag_measurement( + magnetometer_noise: &MagnetometerNoise, + ) -> OwnedVector3Observation + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // Measurement vector + let measurement = + MeasurementVectorBuffer::::new(MatrixData::new_array::< + VEC3_OBSERVATIONS, + 1, + VEC3_OBSERVATIONS, + T, + >( + [zero; VEC3_OBSERVATIONS] + )); + + // Observation matrix + let mut observation_matrix = + ObservationMatrixMutBuffer::::new( + MatrixData::new_array::( + [zero; { VEC3_OBSERVATIONS * STATES }], + ), + ); + observation_matrix.set_all(T::zero()); + + // Measurement noise covariance + let mut noise_covariance = + MeasurementNoiseCovarianceMatrixBuffer::::new( + MatrixData::new_array::< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >([zero; { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }]), + ); + noise_covariance.apply(|mat| { + mat.set_at(0, 0, magnetometer_noise.x); + mat.set_at(1, 1, magnetometer_noise.y); + mat.set_at(2, 2, magnetometer_noise.z); + }); + + // Innovation vector + let innovation_vector = + InnovationVectorBuffer::::new(MatrixData::new_array::< + VEC3_OBSERVATIONS, + 1, + VEC3_OBSERVATIONS, + T, + >( + [zero; VEC3_OBSERVATIONS] + )); + + // Innovation covariance matrix + let innovation_covariance = + InnovationCovarianceMatrixBuffer::::new( + MatrixData::new_array::< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >([zero; { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }]), + ); + + // Kalman Gain matrix + let kalman_gain = KalmanGainMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { STATES * VEC3_OBSERVATIONS }], + ), + ); + + // Temporary residual covariance inverted matrix + let temp_sinv = + TemporaryResidualCovarianceInvertedMatrixBuffer::::new( + MatrixData::new_array::< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >([zero; { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }]), + ); + + // Temporary H×P matrix + let temp_hp = TemporaryHPMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { VEC3_OBSERVATIONS * STATES }], + ), + ); + + // Temporary P×Hᵀ matrix + let temp_pht = TemporaryPHTMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { STATES * VEC3_OBSERVATIONS }], + ), + ); + + // Temporary K×(H×P) matrix + let temp_khp = TemporaryKHPMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + ExtendedObservationBuilder::new::( + observation_matrix, + measurement, + noise_covariance, + innovation_vector, + innovation_covariance, + kalman_gain, + temp_sinv, + temp_hp, + temp_pht, + temp_khp, + ) + } + + /// Builds the Kalman filter observation used for the prediction. + fn build_accel_measurement( + accelerometer_noise: &AccelerometerNoise, + ) -> OwnedVector3Observation + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // Measurement vector + let measurement = + MeasurementVectorBuffer::::new(MatrixData::new_array::< + VEC3_OBSERVATIONS, + 1, + VEC3_OBSERVATIONS, + T, + >( + [zero; VEC3_OBSERVATIONS] + )); + + // Observation matrix + let mut observation_matrix = + ObservationMatrixMutBuffer::::new( + MatrixData::new_array::( + [zero; { VEC3_OBSERVATIONS * STATES }], + ), + ); + observation_matrix.set_all(T::zero()); + + // Measurement noise covariance + let mut noise_covariance = + MeasurementNoiseCovarianceMatrixBuffer::::new( + MatrixData::new_array::< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >([zero; { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }]), + ); + noise_covariance.apply(|mat| { + mat.set_at(0, 0, accelerometer_noise.x); + mat.set_at(1, 1, accelerometer_noise.y); + mat.set_at(2, 2, accelerometer_noise.z); + }); + + // Innovation vector + let innovation_vector = + InnovationVectorBuffer::::new(MatrixData::new_array::< + VEC3_OBSERVATIONS, + 1, + VEC3_OBSERVATIONS, + T, + >( + [zero; VEC3_OBSERVATIONS] + )); + + // Innovation covariance matrix + let innovation_covariance = + InnovationCovarianceMatrixBuffer::::new( + MatrixData::new_array::< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >([zero; { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }]), + ); + + // Kalman Gain matrix + let kalman_gain = KalmanGainMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { STATES * VEC3_OBSERVATIONS }], + ), + ); + + // Temporary residual covariance inverted matrix + let temp_sinv = + TemporaryResidualCovarianceInvertedMatrixBuffer::::new( + MatrixData::new_array::< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >([zero; { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }]), + ); + + // Temporary H×P matrix + let temp_hp = TemporaryHPMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { VEC3_OBSERVATIONS * STATES }], + ), + ); + + // Temporary P×Hᵀ matrix + let temp_pht = TemporaryPHTMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { STATES * VEC3_OBSERVATIONS }], + ), + ); + + // Temporary K×(H×P) matrix + let temp_khp = TemporaryKHPMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + ExtendedObservationBuilder::new::( + observation_matrix, + measurement, + noise_covariance, + innovation_vector, + innovation_covariance, + kalman_gain, + temp_sinv, + temp_hp, + temp_pht, + temp_khp, + ) + } +} diff --git a/src/gyro_free/types.rs b/src/gyro_free/types.rs new file mode 100644 index 0000000..a162f79 --- /dev/null +++ b/src/gyro_free/types.rs @@ -0,0 +1,97 @@ +use minikalman::buffers::types::*; +use minikalman::extended::{ExtendedKalman, ExtendedObservation}; +use minikalman::prelude::*; + +pub const STATES: usize = 4; // quaternion components +pub const VEC3_OBSERVATIONS: usize = 3; // x, y, z + +// A Kalman filter of four states, using owned buffers. +pub type OwnedKalmanFilter = ExtendedKalman< + STATES, + T, + StateTransitionMatrixMutBuffer< + STATES, + T, + MatrixDataArray, + >, + StateVectorBuffer>, + EstimateCovarianceMatrixBuffer< + STATES, + T, + MatrixDataArray, + >, + DirectProcessNoiseCovarianceMatrixMutBuffer< + STATES, + T, + MatrixDataArray, + >, + PredictedStateEstimateVectorBuffer>, + TemporaryStateMatrixBuffer>, +>; + +/// On observation of three states, using owned buffers. +pub type OwnedVector3Observation = ExtendedObservation< + STATES, + VEC3_OBSERVATIONS, + T, + ObservationMatrixMutBuffer< + VEC3_OBSERVATIONS, + STATES, + T, + MatrixDataArray, + >, + MeasurementVectorBuffer< + VEC3_OBSERVATIONS, + T, + MatrixDataArray, + >, + MeasurementNoiseCovarianceMatrixBuffer< + VEC3_OBSERVATIONS, + T, + MatrixDataArray< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >, + >, + InnovationVectorBuffer< + VEC3_OBSERVATIONS, + T, + MatrixDataArray, + >, + InnovationCovarianceMatrixBuffer< + VEC3_OBSERVATIONS, + T, + MatrixDataArray, + >, + KalmanGainMatrixBuffer< + STATES, + VEC3_OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryResidualCovarianceInvertedMatrixBuffer< + VEC3_OBSERVATIONS, + T, + MatrixDataArray< + VEC3_OBSERVATIONS, + VEC3_OBSERVATIONS, + { VEC3_OBSERVATIONS * VEC3_OBSERVATIONS }, + T, + >, + >, + TemporaryHPMatrixBuffer< + VEC3_OBSERVATIONS, + STATES, + T, + MatrixDataArray, + >, + TemporaryPHTMatrixBuffer< + STATES, + VEC3_OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryKHPMatrixBuffer>, +>; diff --git a/src/gyroscope_bias.rs b/src/gyroscope_bias.rs new file mode 100644 index 0000000..d863657 --- /dev/null +++ b/src/gyroscope_bias.rs @@ -0,0 +1,120 @@ +use crate::impl_standard_traits; +use core::fmt::{Debug, Formatter}; +use core::ops::{Mul, Sub}; +use uniform_array_derive::UniformArray; + +#[derive(UniformArray)] +#[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] +#[repr(C)] +pub struct GyroscopeBias { + /// The angular rate around the x-axis, in radians per second. + pub omega_x: T, + /// The angular rate around the y-axis, in radians per second. + pub omega_y: T, + /// The angular rate around the z-axis, in radians per second. + pub omega_z: T, +} + +impl GyroscopeBias { + /// Initializes a new [`GyroscopeBias`] instance. + #[inline(always)] + pub const fn new(omega_x: T, omega_y: T, omega_z: T) -> Self { + Self { + omega_x, + omega_y, + omega_z, + } + } +} + +impl Default for GyroscopeBias +where + T: Default, +{ + #[inline] + fn default() -> Self { + Self::new(Default::default(), Default::default(), Default::default()) + } +} + +impl Clone for GyroscopeBias +where + T: Clone, +{ + fn clone(&self) -> Self { + Self { + omega_x: self.omega_x.clone(), + omega_y: self.omega_y.clone(), + omega_z: self.omega_z.clone(), + } + } +} + +impl Debug for GyroscopeBias +where + T: Debug, +{ + fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result { + f.debug_tuple("GyroscopeBias") + .field(&self.omega_x) + .field(&self.omega_y) + .field(&self.omega_z) + .finish() + } +} + +impl Mul for GyroscopeBias +where + T: Mul + Clone, +{ + type Output = GyroscopeBias; + + fn mul(self, rhs: T) -> Self::Output { + Self { + omega_x: self.omega_x * rhs.clone(), + omega_y: self.omega_y * rhs.clone(), + omega_z: self.omega_z * rhs.clone(), + } + } +} + +impl Sub for GyroscopeBias +where + T: Sub + Clone, +{ + type Output = GyroscopeBias; + + fn sub(self, rhs: T) -> Self::Output { + Self { + omega_x: self.omega_x - rhs.clone(), + omega_y: self.omega_y - rhs.clone(), + omega_z: self.omega_z - rhs.clone(), + } + } +} + +impl_standard_traits!(GyroscopeBias, T); + +#[cfg(test)] +mod test { + use super::*; + + #[test] + fn test_len() { + let reading = GyroscopeBias::::default(); + assert_eq!(reading.len(), 3); + } + + #[test] + fn test_index() { + let reading = GyroscopeBias:: { + omega_x: 1.0, + omega_y: 2.0, + omega_z: 3.0, + }; + + assert_eq!(reading[0], 1.0); + assert_eq!(reading[1], 2.0); + assert_eq!(reading[2], 3.0); + } +} diff --git a/src/gyroscope_noise.rs b/src/gyroscope_noise.rs new file mode 100644 index 0000000..f1b3160 --- /dev/null +++ b/src/gyroscope_noise.rs @@ -0,0 +1,101 @@ +use crate::impl_standard_traits; +use core::fmt::{Debug, Formatter}; +use core::ops::Mul; +use uniform_array_derive::UniformArray; + +#[derive(UniformArray)] +#[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] +#[repr(C)] +pub struct GyroscopeNoise { + /// The rotation rate noise along the x-axis. + pub x: T, + /// The rotation rate noise along the y-axis. + pub y: T, + /// The rotation rate noise along the z-axis. + pub z: T, +} + +impl GyroscopeNoise { + /// Initializes a new [`GyroscopeNoise`] instance. + #[inline(always)] + pub const fn new(x: T, y: T, z: T) -> Self { + Self { x, y, z } + } +} + +impl Default for GyroscopeNoise +where + T: Default, +{ + #[inline] + fn default() -> Self { + Self::new(Default::default(), Default::default(), Default::default()) + } +} + +impl Clone for GyroscopeNoise +where + T: Clone, +{ + fn clone(&self) -> Self { + Self { + x: self.x.clone(), + y: self.y.clone(), + z: self.z.clone(), + } + } +} + +impl Debug for GyroscopeNoise +where + T: Debug, +{ + fn fmt(&self, f: &mut Formatter<'_>) -> core::fmt::Result { + f.debug_tuple("GyroscopeNoise") + .field(&self.x) + .field(&self.y) + .field(&self.z) + .finish() + } +} + +impl Mul for GyroscopeNoise +where + T: Mul + Clone, +{ + type Output = GyroscopeNoise; + + fn mul(self, rhs: T) -> Self::Output { + Self { + x: self.x * rhs.clone(), + y: self.y * rhs.clone(), + z: self.z * rhs.clone(), + } + } +} + +impl_standard_traits!(GyroscopeNoise, T); + +#[cfg(test)] +mod test { + use super::*; + + #[test] + fn test_len() { + let reading = GyroscopeNoise::::default(); + assert_eq!(reading.len(), 3); + } + + #[test] + fn test_index() { + let reading = GyroscopeNoise:: { + x: 1.0, + y: 2.0, + z: 3.0, + }; + + assert_eq!(reading[0], 1.0); + assert_eq!(reading[1], 2.0); + assert_eq!(reading[2], 3.0); + } +} diff --git a/src/gyroscope_reading.rs b/src/gyroscope_reading.rs index 9eb6470..597c1cd 100644 --- a/src/gyroscope_reading.rs +++ b/src/gyroscope_reading.rs @@ -1,7 +1,9 @@ use crate::impl_standard_traits; use core::fmt::{Debug, Formatter}; -use core::ops::Mul; +use core::ops::{Mul, Sub}; +use uniform_array_derive::UniformArray; +#[derive(UniformArray)] #[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] #[repr(C)] pub struct GyroscopeReading { @@ -24,11 +26,20 @@ impl GyroscopeReading { } } - /// Returns the length of the [`GyroscopeReading`] vector. - #[inline(always)] - #[allow(unused)] - pub const fn len(&self) -> usize { - 3 + /// Constructs a new [`GyroscopeReading`] instance from a reading in a given coordinate frame. + #[cfg(feature = "coordinate-frame")] + #[cfg_attr(docsrs, doc(cfg(feature = "coordinate-frame")))] + pub fn north_east_down(coordinate: C) -> Self + where + C: Into>, + T: Clone, + { + let coordinate = coordinate.into(); + Self { + omega_x: coordinate.x(), + omega_y: coordinate.y(), + omega_z: coordinate.z(), + } } } @@ -83,31 +94,30 @@ where } } -#[cfg(not(feature = "unsafe"))] -impl core::ops::Index for GyroscopeReading { - type Output = T; +impl Sub for GyroscopeReading +where + T: Sub + Clone, +{ + type Output = GyroscopeReading; - #[inline(always)] - fn index(&self, index: usize) -> &Self::Output { - match index { - 0 => &self.omega_x, - 1 => &self.omega_y, - 2 => &self.omega_z, - _ => panic!("Index out of bounds"), + fn sub(self, rhs: T) -> Self::Output { + Self { + omega_x: self.omega_x - rhs.clone(), + omega_y: self.omega_y - rhs.clone(), + omega_z: self.omega_z - rhs.clone(), } } } -#[cfg(not(feature = "unsafe"))] -impl core::ops::IndexMut for GyroscopeReading { - #[inline(always)] - fn index_mut(&mut self, index: usize) -> &mut Self::Output { - match index { - 0 => &mut self.omega_x, - 1 => &mut self.omega_y, - 2 => &mut self.omega_z, - _ => panic!("Index out of bounds"), - } +#[cfg(feature = "coordinate-frame")] +#[cfg_attr(docsrs, doc(cfg(feature = "coordinate-frame")))] +impl From for GyroscopeReading +where + C: coordinate_frame::CoordinateFrame, + T: Copy + coordinate_frame::SaturatingNeg, +{ + fn from(value: C) -> Self { + Self::north_east_down(value.to_ned()) } } diff --git a/src/lib.rs b/src/lib.rs index c2b523f..338277d 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -9,773 +9,27 @@ mod accelerometer_noise; mod accelerometer_reading; mod euler_angles; +pub mod gyro_drift; +pub mod gyro_free; +mod gyroscope_bias; +mod gyroscope_noise; mod gyroscope_reading; mod macros; mod magnetometer_noise; mod magnetometer_reading; mod num_traits; +#[deprecated(note = "This is a test implementation not suitable for real use")] +pub mod test_estimator; mod vector3; -use crate::accelerometer_noise::AccelerometerNoise; -use crate::accelerometer_reading::AccelerometerReading; -use crate::euler_angles::EulerAngles; -use crate::magnetometer_noise::MagnetometerNoise; -use crate::magnetometer_reading::MagnetometerReading; -use crate::vector3::Vector3; -use core::ops::Neg; -use minikalman::buffers::types::*; -use minikalman::prelude::*; -use minikalman::regular::{ - Control, ControlBuilder, RegularKalman, RegularKalmanBuilder, RegularObservation, - RegularObservationBuilder, -}; +pub use crate::accelerometer_noise::AccelerometerNoise; +pub use crate::accelerometer_reading::AccelerometerReading; +pub use crate::euler_angles::EulerAngles; +pub use crate::gyroscope_bias::GyroscopeBias; +pub use crate::gyroscope_noise::GyroscopeNoise; +pub use crate::gyroscope_reading::GyroscopeReading; +pub use crate::magnetometer_noise::MagnetometerNoise; +pub use crate::magnetometer_reading::MagnetometerReading; +pub use crate::vector3::Vector3; pub use crate::num_traits::*; - -const STATES: usize = 3; // roll rate, pitch rate, yaw rate -const CONTROLS: usize = 3; // roll rate, pitch rate, yaw rate -const OBSERVATIONS: usize = 3; // roll, pitch, yaw - -/// A MARG (Magnetic, Angular Rate, and Gravity) orientation estimator. -pub struct OrientationEstimator {} - -/// An owned orientation estimator with generic type `T`. -pub struct OwnedOrientationEstimator { - filter: OwnedKalmanFilter, - control: OwnedControlInput, - measurement: OwnedObservation, - /// The angular tolerance, in radians, to detect a Gimbal Lock. - gimbal_lock_tolerance: T, - /// The autocovariances of the noise terms. - accelerometer_noise: AccelerometerNoise, - /// The autocovariances of the noise terms. - magnetometer_noise: MagnetometerNoise, - /// A bias term to avoid divisions by zero. - epsilon: T, -} - -impl OwnedOrientationEstimator { - /// Initializes a new instance of the [`OwnedOrientationEstimator`] struct. - /// - /// ## Arguments - /// * `gimbal_lock_tolerance` - The angular tolerance, in radians, to detect a Gimbal Lock. - /// * `accelerometer_noise` - The accelerometer noise values (sigma-squared) for each axis. - /// * `magnetometer_noise` - The magnetometer noise values (sigma-squared) for each axis. - /// * `epsilon` - A small bias term to avoid divisions by zero. Set to e.g. `1e-6`. - pub fn new( - gimbal_lock_tolerance: T, - accelerometer_noise: AccelerometerNoise, - magnetometer_noise: MagnetometerNoise, - epsilon: T, - ) -> Self - where - T: MatrixDataType + Default, - { - let filter = Self::build_filter(); - let control = Self::build_control(); - let measurement = Self::build_measurement(); - - Self { - filter, - control, - measurement, - gimbal_lock_tolerance, - accelerometer_noise, - magnetometer_noise, - epsilon, - } - } -} - -impl OwnedOrientationEstimator { - /// Performs a prediction step to obtain new orientation estimates. - /// - /// ## Arguments - /// * `delta_t` - The time step for the prediction. - /// * `angular_rates` - The angular rates measured by the magnetometer. - pub fn predict(&mut self, delta_t: T, angular_rates: &MagnetometerReading) - where - T: MatrixDataType + Default, - { - // Update the Kalman filter components. - self.update_state_transition_matrix(delta_t, angular_rates); - self.update_control_input(delta_t, angular_rates); - - // Perform a regular Kalman Filter prediction step. - self.filter.predict(); - self.filter.control(&mut self.control); - - // TODO: Clamp any angles? - } - - /// Performs a correction step using accelerometer and magnetometer readings. - /// - /// ## Arguments - /// * `accelerometer` - The accelerometer reading. - /// * `magnetometer` - The magnetometer reading. - pub fn correct( - &mut self, - accelerometer: &AccelerometerReading, - magnetometer: &MagnetometerReading, - ) where - T: MatrixDataType - + ArcSin - + ArcTan - + DetectGimbalLock - + core::fmt::Debug, - { - // Normalize the vectors. - let a = Vector3::from(accelerometer).normalized(); - let m = Vector3::from(magnetometer).normalized(); - - // Obtain the Euler Angles and apply them to the measurement. - let angles = self.build_triad(a, m); - self.measurement.measurement_vector_mut().apply(|vec| { - vec.set_row(0, angles.roll_phi); - vec.set_row(1, angles.pitch_theta); - vec.set_row(2, angles.yaw_psi); - }); - - // Update the measurement noise. - self.update_measurement_noise(a, m); - - // Perform the update step. - self.filter.correct(&mut self.measurement); - - // TODO: Clamp any angles? - } - - /// Computes the Jacobian matrices and measurement noise matrix for orientation estimation - /// - /// This is done by propagating the sensor noise through the TRIAD method - /// The final measurement noise covariance matrix R is computed using J_a, R_a, J_m, and R_m - /// R represents the combined effect of accelerometer and magnetometer noise on the orientation estimates, - /// where J_a and J_m are the Jacobians of the respective measurement noise. - /// - /// ## Arguments - /// * `accelerometer` - The normalized accelerometer vector. - /// * `magnetometer` - The normalized magnetometer vector. - fn update_measurement_noise(&mut self, accelerometer: Vector3, magnetometer: Vector3) - where - T: MatrixDataType + core::fmt::Debug, - { - // Easy access to accelerometer noise. - let sa11 = self.accelerometer_noise.x; // sigma_a_xx - let sa22 = self.accelerometer_noise.y; // sigma_a_yy - let sa33 = self.accelerometer_noise.z; // sigma_a_zz - - // Easy access to magnetometer noise. - let sm11 = self.magnetometer_noise.x; // sigma_m_xx - let sm22 = self.magnetometer_noise.y; // sigma_m_yy - let sm33 = self.magnetometer_noise.z; // sigma_m_zz - - // Helper "constants". - let two = T::one() + T::one(); - let epsilon = self.epsilon; - - // Easy access to accelerometer components. - let ax = accelerometer.x; - let ay = accelerometer.y; - let az = accelerometer.z; - - // Easy access to magnetometer components. - let mx = magnetometer.x; - let my = magnetometer.y; - let mz = magnetometer.z; - - // Common squares of the accelerometer components. - let ax2 = accelerometer.x * accelerometer.x; - let ay2 = accelerometer.y * accelerometer.y; - let az2 = accelerometer.z * accelerometer.z; - - // Common squares of the magnetometer components. - let mx2 = magnetometer.x * magnetometer.x; - let my2 = magnetometer.y * magnetometer.y; - let mz2 = magnetometer.z * magnetometer.z; - - // Noise matrix. - let r = self.measurement.measurement_noise_covariance_mut(); - - // Common denominator of the matrix components. - #[rustfmt::skip] - let denom = - ax2 * az2 * mx2 - + ax2 * my2 - + two * ax2 * ay * az2 * mx * my - - two * ax * ay * mx * my - + two * ax * az2 * az * mx * mz - - two * ax * az * mx * mz - + ay2 * az2 * my2 - + ay2 * mx2 - + two * ay * az2 * az * my * mz - - two * ay * az * my * mz - + az2 * az2 * mz2 - - two * az2 * mz2 - + mz2; - - let one_minus_az2 = T::one() - az2; - let one_minus_az2_sqrt = one_minus_az2.square_root(); - - // Row 1, column 1. - let r11_nom = sa11 * sq(ay * az * mx2 + ay * az * my2 + az2 * my * mz - my * mz) - + sa22 * sq(ax * az * mx2 + ax * az * my2 + az2 * mx * mz - mx * mz) - + sa33 - * sq( - ax2 * mx * my - ax * my * mx2 + ax * ay * my2 + two * ax * az * my * mz - - ay2 * mx * my - - two * ay * az * mx * mz, - ) - + sm11 * sq(ax2 * az * my + ay2 * az * my + ay * az2 * mz - ay * mz) - + sm22 * sq(ax2 * az * mx + ax * az2 * mz - ax * mz + ay2 * az * mx) - + sm33 * sq(ax * az2 * my - ax * my - ay * az2 * mx + ay * mx); - let r11 = r11_nom / (sq(denom) + epsilon); - r.set_at(0, 0, r11); - - // Row 1, column 2 and row 2, column 1. - let r12_nom = sa33 - * (-ax2 * mx * my + ax * ay * mx2 - ax * ay * my2 - two * ax * az * my * mz - + ay2 * mx * my - + two * ay * az * mx * mz); - let r12 = r12_nom / (one_minus_az2_sqrt * denom + epsilon); - r.set_symmetric(0, 1, r12); - - // Row 1, column 3 and row 3, column 1. - let r13_nom = ax * sa22 * (ax * az * mx2 + ax * az * my2 + az2 * mx * mz - mx * mz) - + ay * sa11 * (ay * az * mx2 + ay * az * my2 + az2 * my * mz - my * mz); - let r13 = r13_nom / ((ax2 + ay2) * denom + epsilon); - r.set_symmetric(0, 2, r13); - - // Row 2, column 2. - let r22 = -sa33 / (ax2 - T::one() + epsilon); - r.set_at(1, 1, r22); - - // Row 2, column 3 and row 3, column 2. - r.set_symmetric(1, 2, T::zero()); - - // Row 3, column 3. - let r33 = (ax2 * sa22 + ay2 * sa11) / sq(ax2 + ay2 + epsilon); - r.set_at(2, 2, r33); - } - - /// Constructs the state transition matrix based on the angular rates. - fn update_state_transition_matrix(&mut self, delta_t: T, angular_rates: &MagnetometerReading) - where - T: MatrixDataType + Default, - { - let rates = *angular_rates * delta_t; - self.filter.state_transition_mut().apply(|mat| { - mat.set_at(0, 0, T::one()); - mat.set_at(0, 1, -rates.z); - mat.set_at(0, 2, rates.y); - - mat.set_at(1, 0, rates.z); - mat.set_at(1, 1, T::one()); - mat.set_at(1, 2, -rates.x); - - mat.set_at(2, 0, -rates.y); - mat.set_at(2, 1, rates.x); - mat.set_at(2, 2, T::one()); - }); - } - - /// Constructs the state transition matrix based on the angular rates. - fn update_control_input(&mut self, delta_t: T, angular_rates: &MagnetometerReading) - where - T: MatrixDataType + Default, - { - let rates = *angular_rates * delta_t; - self.control.control_vector_mut().apply(|vec| { - vec.set_row(0, rates.x); - vec.set_row(1, rates.y); - vec.set_row(2, rates.z); - }); - } - - /// Builds the TRIAD rotation matrix and obtains the Euler angles from it. - /// - /// ## Arguments - /// * `a` - The normalized accelerometer vector. - /// * `m` - The normalized magnetometer vector. - fn build_triad(&self, a: Vector3, m: Vector3) -> EulerAngles - where - T: MatrixDataType - + ArcTan - + ArcSin - + Neg - + DetectGimbalLock, - { - // Calculate TRIAD vectors. - let b1 = a; - let b2 = (m - a * (m * a)).normalized(); - let b3 = b2.cross(b1); - // TRIAD rotation matrix: [b1, b2, b3], stacked columns - - // Derive Euler angles from TRIAD rotation matrix (ZYX sequence). - let theta = -ArcSin::arcsin(b1.z); // pitch - - // Handle Gimbal lock situations. - if theta.close_to_zenith_or_nadir(self.gimbal_lock_tolerance) { - let psi = ArcTan::atan2(-b3.y, b2.y); // yaw - let phi = T::zero(); // roll - EulerAngles::new(phi, theta, psi) - } else { - let psi = ArcTan::atan2(b1.y, b1.x); // yaw - let phi = ArcTan::atan2(b2.z, b3.z); // roll - EulerAngles::new(phi, theta, psi) - } - } -} - -/// Calculates the square of the `value`. -#[inline] -fn sq(value: T) -> T -where - T: core::ops::Mul + Copy, -{ - value * value -} - -impl OwnedOrientationEstimator { - /// Builds the Kalman filter used for prediction. - fn build_filter() -> OwnedKalmanFilter - where - T: MatrixDataType + Default, - { - let zero = T::default(); - - // State vector. - let state_vec = - StateVectorBuffer::::new(MatrixData::new_array::( - [zero; STATES], - )); - - // State transition matrix. - let state_transition = - StateTransitionMatrixMutBuffer::::new(MatrixData::new_array::< - STATES, - STATES, - { STATES * STATES }, - T, - >( - [zero; { STATES * STATES }] - )); - - // Estimate covariance matrix. - let mut estimate_covariance = - EstimateCovarianceMatrixBuffer::::new(MatrixData::new_array::< - STATES, - STATES, - { STATES * STATES }, - T, - >( - [zero; { STATES * STATES }] - )); - estimate_covariance.make_identity(); - - // Process noise matrix. - let process_noise = DirectProcessNoiseCovarianceMatrixMutBuffer::::new( - MatrixData::new_array::( - [zero; { STATES * STATES }], - ), - ); - - // Predicted state vector. - let mut predicted_state = - PredictedStateEstimateVectorBuffer::::new(MatrixData::new_array::< - STATES, - 1, - STATES, - T, - >([zero; STATES])); - predicted_state.set_all(T::one()); - - // Temporary estimate covariance matrix. - let temp_state_matrix = - TemporaryStateMatrixBuffer::::new(MatrixData::new_array::< - STATES, - STATES, - { STATES * STATES }, - T, - >( - [zero; { STATES * STATES }] - )); - - RegularKalmanBuilder::new::( - state_transition, - state_vec, - estimate_covariance, - process_noise, - predicted_state, - temp_state_matrix, - ) - } - - /// Builds the Kalman filter control input. - fn build_control() -> OwnedControlInput - where - T: MatrixDataType + Default, - { - let zero = T::default(); - - // Control vector. - let control_vector = ControlVectorBuffer::::new(MatrixData::new_array::< - CONTROLS, - 1, - CONTROLS, - T, - >([zero; CONTROLS])); - - // Control matrix. - let mut control_matrix = - ControlMatrixMutBuffer::::new(MatrixData::new_array::< - STATES, - CONTROLS, - { STATES * CONTROLS }, - T, - >( - [zero; STATES * CONTROLS] - )); - control_matrix.make_identity(); - - // Process noise matrix. - let process_noise = ControlProcessNoiseCovarianceMatrixBuffer::::new( - MatrixData::new_array::( - [zero; CONTROLS * CONTROLS], - ), - ); - - // Temporary matrix. - let temp = TemporaryBQMatrixBuffer::::new(MatrixData::new_array::< - STATES, - CONTROLS, - { STATES * CONTROLS }, - T, - >( - [zero; STATES * CONTROLS], - )); - - ControlBuilder::new::( - control_matrix, - control_vector, - process_noise, - temp, - ) - } - - /// Builds the Kalman filter observation used for the prediction. - fn build_measurement() -> OwnedObservation - where - T: MatrixDataType + Default, - { - let zero = T::default(); - - // Measurement vector - let measurement = - MeasurementVectorBuffer::::new(MatrixData::new_array::< - OBSERVATIONS, - 1, - OBSERVATIONS, - T, - >([zero; OBSERVATIONS])); - - // Observation matrix - let mut observation_matrix = - ObservationMatrixMutBuffer::::new(MatrixData::new_array::< - OBSERVATIONS, - STATES, - { OBSERVATIONS * STATES }, - T, - >( - [zero; { OBSERVATIONS * STATES }], - )); - observation_matrix.make_identity(); - - // Measurement noise covariance - let noise_covariance = MeasurementNoiseCovarianceMatrixBuffer::::new( - MatrixData::new_array::( - [zero; { OBSERVATIONS * OBSERVATIONS }], - ), - ); - // TODO: Apply measurement noise! - - // Innovation vector - let innovation_vector = - InnovationVectorBuffer::::new(MatrixData::new_array::< - OBSERVATIONS, - 1, - OBSERVATIONS, - T, - >([zero; OBSERVATIONS])); - - // Innovation covariance matrix - let innovation_covariance = - InnovationCovarianceMatrixBuffer::::new(MatrixData::new_array::< - OBSERVATIONS, - OBSERVATIONS, - { OBSERVATIONS * OBSERVATIONS }, - T, - >( - [zero; { OBSERVATIONS * OBSERVATIONS }], - )); - - // Kalman Gain matrix - let kalman_gain = - KalmanGainMatrixBuffer::::new(MatrixData::new_array::< - STATES, - OBSERVATIONS, - { STATES * OBSERVATIONS }, - T, - >( - [zero; { STATES * OBSERVATIONS }], - )); - - // Temporary residual covariance inverted matrix - let temp_sinv = TemporaryResidualCovarianceInvertedMatrixBuffer::::new( - MatrixData::new_array::( - [zero; { OBSERVATIONS * OBSERVATIONS }], - ), - ); - - // Temporary H×P matrix - let temp_hp = - TemporaryHPMatrixBuffer::::new(MatrixData::new_array::< - OBSERVATIONS, - STATES, - { OBSERVATIONS * STATES }, - T, - >( - [zero; { OBSERVATIONS * STATES }], - )); - - // Temporary P×Hᵀ matrix - let temp_pht = - TemporaryPHTMatrixBuffer::::new(MatrixData::new_array::< - STATES, - OBSERVATIONS, - { STATES * OBSERVATIONS }, - T, - >( - [zero; { STATES * OBSERVATIONS }], - )); - - // Temporary K×(H×P) matrix - let temp_khp = TemporaryKHPMatrixBuffer::::new(MatrixData::new_array::< - STATES, - STATES, - { STATES * STATES }, - T, - >( - [zero; { STATES * STATES }] - )); - - RegularObservationBuilder::new::( - observation_matrix, - measurement, - noise_covariance, - innovation_vector, - innovation_covariance, - kalman_gain, - temp_sinv, - temp_hp, - temp_pht, - temp_khp, - ) - } -} - -type OwnedKalmanFilter = RegularKalman< - STATES, - T, - StateTransitionMatrixMutBuffer< - STATES, - T, - MatrixDataArray, - >, - StateVectorBuffer>, - EstimateCovarianceMatrixBuffer< - STATES, - T, - MatrixDataArray, - >, - DirectProcessNoiseCovarianceMatrixMutBuffer< - STATES, - T, - MatrixDataArray, - >, - PredictedStateEstimateVectorBuffer>, - TemporaryStateMatrixBuffer>, ->; - -type OwnedControlInput = Control< - STATES, - CONTROLS, - T, - ControlMatrixMutBuffer>, - ControlVectorBuffer>, - ControlProcessNoiseCovarianceMatrixBuffer< - CONTROLS, - T, - MatrixDataArray, - >, - TemporaryBQMatrixBuffer< - STATES, - CONTROLS, - T, - MatrixDataArray, - >, ->; - -type OwnedObservation = RegularObservation< - STATES, - OBSERVATIONS, - T, - ObservationMatrixMutBuffer< - OBSERVATIONS, - STATES, - T, - MatrixDataArray, - >, - MeasurementVectorBuffer>, - MeasurementNoiseCovarianceMatrixBuffer< - OBSERVATIONS, - T, - MatrixDataArray, - >, - InnovationVectorBuffer>, - InnovationCovarianceMatrixBuffer< - OBSERVATIONS, - T, - MatrixDataArray, - >, - KalmanGainMatrixBuffer< - STATES, - OBSERVATIONS, - T, - MatrixDataArray, - >, - TemporaryResidualCovarianceInvertedMatrixBuffer< - OBSERVATIONS, - T, - MatrixDataArray, - >, - TemporaryHPMatrixBuffer< - OBSERVATIONS, - STATES, - T, - MatrixDataArray, - >, - TemporaryPHTMatrixBuffer< - STATES, - OBSERVATIONS, - T, - MatrixDataArray, - >, - TemporaryKHPMatrixBuffer>, ->; - -#[cfg(test)] -#[cfg(feature = "std")] -mod tests { - use super::*; - use crate::accelerometer_reading::AccelerometerReading; - - trait Round { - /// Rounds to two decimal places. - fn round2(self) -> T; - - /// Rounds to seven decimal places. - fn round7(self) -> T; - } - - impl Round for f32 { - fn round2(self) -> f32 { - (self * 100.0).round() * 0.01 - } - - fn round7(self) -> f32 { - (self * 1000000.0).round() * 0.000001 - } - } - - #[test] - fn test_state_transition_matrix() { - let gimbal_lock_tolerance = 0.01; - let accelerometer_noise = AccelerometerNoise::new(0.01, 0.02, 0.03); - let magnetometer_noise = MagnetometerNoise::new(0.04, 0.05, 0.06); - let epsilon = 1e-6; - - let mut estimator: OwnedOrientationEstimator = OwnedOrientationEstimator::new( - gimbal_lock_tolerance, - accelerometer_noise, - magnetometer_noise, - epsilon, - ); - - // Set up example error covariances (sigma phi/theta/psi). - estimator.filter.estimate_covariance_mut().make_scalar(0.1); - - // Set up example process noise. - // TODO: Pass as argument - estimator - .filter - .direct_process_noise_mut() - .make_scalar(0.01); - - // Set up example measurement noise. - // TODO: Pass as argument - estimator - .measurement - .measurement_noise_covariance_mut() - .make_scalar(0.01); - - let delta_t: f32 = 0.1; - let angular_rates = MagnetometerReading::new(0.01, 0.02, -0.01); - estimator.predict(delta_t, &angular_rates); - - estimator.filter.state_transition().inspect(|mat| { - assert_eq!(mat.get_at(0, 0), 1.0); - assert_eq!(mat.get_at(0, 1), 0.001); - assert_eq!(mat.get_at(0, 2), 0.002); - - assert_eq!(mat.get_at(1, 0), -0.001); - assert_eq!(mat.get_at(1, 1), 1.0); - assert_eq!(mat.get_at(1, 2), -0.001); - - assert_eq!(mat.get_at(2, 0), -0.002); - assert_eq!(mat.get_at(2, 1), 0.001); - assert_eq!(mat.get_at(2, 2), 1.0); - }); - - estimator.filter.state_vector().inspect(|vec| { - assert_eq!(vec.get_row(0), 0.001); - assert_eq!(vec.get_row(1), 0.002); - assert_eq!(vec.get_row(2), -0.001); - }); - - estimator.filter.estimate_covariance().inspect(|mat| { - assert_eq!(mat.get_at(0, 0).round2(), 0.11); - assert_eq!(mat.get_at(0, 1).round2(), -0.0); - assert_eq!(mat.get_at(0, 2).round2(), 0.0); - - assert_eq!(mat.get_at(1, 0).round2(), -0.0); - assert_eq!(mat.get_at(1, 1).round2(), 0.11); - assert_eq!(mat.get_at(1, 2).round2(), 0.0); - - assert_eq!(mat.get_at(2, 0).round2(), 0.0); - assert_eq!(mat.get_at(2, 1).round2(), 0.0); - assert_eq!(mat.get_at(2, 2).round2(), 0.11); - }); - - // Incorporate accelerometer vector. - let accelerometer = AccelerometerReading::new(0.0, 0.0, -9.81); - let magnetometer = MagnetometerReading::new(0.2, 0.0, 0.4); - - estimator.correct(&accelerometer, &magnetometer); - - estimator.filter.state_vector().inspect(|vec| { - assert_eq!(vec.get_row(0).round7(), 0.0); // replace with approximate tests - assert_eq!(vec.get_row(1).round2(), 1.23); // replace with approximate tests - assert_eq!(vec.get_row(2).round2(), -1.5699999); // replace with approximate tests - }) - } -} diff --git a/src/macros.rs b/src/macros.rs index 0728a5e..d88722d 100644 --- a/src/macros.rs +++ b/src/macros.rs @@ -2,134 +2,5 @@ macro_rules! impl_standard_traits { ($type_name:ident, $type_param:ident) => { impl<$type_param> Copy for $type_name<$type_param> where $type_param: Copy {} - - #[cfg_attr(docsrs, doc(cfg(feature = "unsafe")))] - #[cfg(feature = "unsafe")] - impl<$type_param> $type_name<$type_param> - where - $type_param: Default, - { - /// Constructs a new instance from a slice. - #[allow(unused)] - #[inline] - pub fn from_slice(slice: &[$type_param]) -> &Self { - core::assert_eq!( - slice.len(), - core::mem::size_of::() / core::mem::size_of::<$type_param>() - ); - - // SAFETY: $type_name only contains `$type_param` fields and is `repr(C)` - unsafe { &*(slice.as_ptr() as *const Self) } - } - - /// Constructs a new instance from a mutable slice. - #[allow(unused)] - #[inline] - pub fn from_mut_slice(slice: &mut [$type_param]) -> &mut Self { - core::assert_eq!( - slice.len(), - core::mem::size_of::() / core::mem::size_of::<$type_param>() - ); - - // SAFETY: $type_name only contains `$type_param` fields and is `repr(C)` - unsafe { &mut *(slice.as_mut_ptr() as *mut Self) } - } - } - - #[cfg_attr(docsrs, doc(cfg(feature = "unsafe")))] - #[cfg(feature = "unsafe")] - impl<$type_param> core::convert::AsRef<[$type_param]> for $type_name<$type_param> - where - $type_param: Default, - { - fn as_ref(&self) -> &[$type_param] { - unsafe { - // SAFETY: $type_name only contains `$type_param` fields and is `repr(C)` - core::slice::from_raw_parts( - self as *const _ as *const $type_param, - core::mem::size_of::<$type_name<$type_param>>() - / core::mem::size_of::<$type_param>(), - ) - } - } - } - - #[cfg_attr(docsrs, doc(cfg(feature = "unsafe")))] - #[cfg(feature = "unsafe")] - impl<$type_param> core::convert::AsMut<[$type_param]> for $type_name<$type_param> - where - $type_param: Default, - { - fn as_mut(&mut self) -> &mut [$type_param] { - unsafe { - // SAFETY: $type_name only contains `$type_param` fields and is `repr(C)` - core::slice::from_raw_parts_mut( - self as *mut _ as *mut $type_param, - core::mem::size_of::<$type_name<$type_param>>() - / core::mem::size_of::<$type_param>(), - ) - } - } - } - - #[cfg_attr(docsrs, doc(cfg(feature = "unsafe")))] - #[cfg(feature = "unsafe")] - impl<$type_param> core::ops::Deref for $type_name<$type_param> - where - $type_param: Default, - { - type Target = [$type_param]; - - fn deref(&self) -> &Self::Target { - self.as_ref() - } - } - - #[cfg_attr(docsrs, doc(cfg(feature = "unsafe")))] - #[cfg(feature = "unsafe")] - impl<$type_param> core::ops::DerefMut for $type_name<$type_param> - where - $type_param: Default, - { - fn deref_mut(&mut self) -> &mut Self::Target { - self.as_mut() - } - } - - #[cfg(test)] - paste::paste! { - #[cfg(test)] - mod [] { - #[cfg(feature = "unsafe")] - use super::*; - - #[test] - #[cfg(feature = "unsafe")] - fn test_from_slice() { - const TYPE_SIZE: usize = core::mem::size_of::<$type_name>(); - const NUM_ELEMS: usize = TYPE_SIZE / core::mem::size_of::(); - const ARRAY_SIZE: usize = NUM_ELEMS + 1; - let data = [0; ARRAY_SIZE]; - let state = $type_name::from_slice(&data[..NUM_ELEMS]); - core::assert_eq!(state.len(), NUM_ELEMS); - core::assert!(core::ptr::eq(state.as_ptr(), data.as_ptr())); - } - - #[test] - #[cfg(feature = "unsafe")] - fn test_from_slice_mut() { - const TYPE_SIZE: usize = core::mem::size_of::<$type_name>(); - const NUM_ELEMS: usize = TYPE_SIZE / core::mem::size_of::(); - const ARRAY_SIZE: usize = NUM_ELEMS + 1; - let mut data = [0; ARRAY_SIZE]; - { - let state = $type_name::from_mut_slice(&mut data[..NUM_ELEMS]); - state[0] = 10; - core::assert!(core::ptr::eq(state.as_ptr(), data.as_ptr())); - } - core::assert_eq!(data[0], 10, "expect data to be changed"); - } - } - } }; } diff --git a/src/magnetometer_noise.rs b/src/magnetometer_noise.rs index 529f3c2..ebcfdfb 100644 --- a/src/magnetometer_noise.rs +++ b/src/magnetometer_noise.rs @@ -1,7 +1,9 @@ use crate::impl_standard_traits; use core::fmt::{Debug, Formatter}; use core::ops::Mul; +use uniform_array_derive::UniformArray; +#[derive(UniformArray)] #[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] #[repr(C)] pub struct MagnetometerNoise { @@ -19,12 +21,6 @@ impl MagnetometerNoise { pub const fn new(x: T, y: T, z: T) -> Self { Self { x, y, z } } - - /// Returns the length of the [`MagnetometerNoise`] vector. - #[inline(always)] - pub const fn len(&self) -> usize { - 3 - } } impl Default for MagnetometerNoise @@ -78,34 +74,6 @@ where } } -#[cfg(not(feature = "unsafe"))] -impl core::ops::Index for MagnetometerNoise { - type Output = T; - - #[inline(always)] - fn index(&self, index: usize) -> &Self::Output { - match index { - 0 => &self.x, - 1 => &self.y, - 2 => &self.z, - _ => panic!("Index out of bounds"), - } - } -} - -#[cfg(not(feature = "unsafe"))] -impl core::ops::IndexMut for MagnetometerNoise { - #[inline(always)] - fn index_mut(&mut self, index: usize) -> &mut Self::Output { - match index { - 0 => &mut self.x, - 1 => &mut self.y, - 2 => &mut self.z, - _ => panic!("Index out of bounds"), - } - } -} - impl_standard_traits!(MagnetometerNoise, T); #[cfg(test)] diff --git a/src/magnetometer_reading.rs b/src/magnetometer_reading.rs index cddde56..77ca840 100644 --- a/src/magnetometer_reading.rs +++ b/src/magnetometer_reading.rs @@ -1,7 +1,9 @@ use crate::impl_standard_traits; use core::fmt::{Debug, Formatter}; use core::ops::Mul; +use uniform_array_derive::UniformArray; +#[derive(UniformArray)] #[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] #[repr(C)] pub struct MagnetometerReading { @@ -20,10 +22,28 @@ impl MagnetometerReading { Self { x, y, z } } - /// Returns the length of the [`MagnetometerReading`] vector. - #[inline(always)] - pub const fn len(&self) -> usize { - 3 + /// Constructs a new [`MagnetometerReading`] instance from a reading in a given coordinate frame. + #[cfg(feature = "coordinate-frame")] + #[cfg_attr(docsrs, doc(cfg(feature = "coordinate-frame")))] + pub fn north_east_down(coordinate: C) -> Self + where + C: Into>, + T: Clone, + { + let coordinate = coordinate.into(); + Self { + x: coordinate.x(), + y: coordinate.y(), + z: coordinate.z(), + } + } + + /// Represents this reading as a tuple. + pub fn as_tuple(&self) -> (T, T, T) + where + T: Copy, + { + (self.x, self.y, self.z) } } @@ -78,31 +98,15 @@ where } } -#[cfg(not(feature = "unsafe"))] -impl core::ops::Index for MagnetometerReading { - type Output = T; - - #[inline(always)] - fn index(&self, index: usize) -> &Self::Output { - match index { - 0 => &self.x, - 1 => &self.y, - 2 => &self.z, - _ => panic!("Index out of bounds"), - } - } -} - -#[cfg(not(feature = "unsafe"))] -impl core::ops::IndexMut for MagnetometerReading { - #[inline(always)] - fn index_mut(&mut self, index: usize) -> &mut Self::Output { - match index { - 0 => &mut self.x, - 1 => &mut self.y, - 2 => &mut self.z, - _ => panic!("Index out of bounds"), - } +#[cfg(feature = "coordinate-frame")] +#[cfg_attr(docsrs, doc(cfg(feature = "coordinate-frame")))] +impl From for MagnetometerReading +where + C: coordinate_frame::CoordinateFrame, + T: Copy + coordinate_frame::SaturatingNeg, +{ + fn from(value: C) -> Self { + Self::north_east_down(value.to_ned()) } } diff --git a/src/num_traits.rs b/src/num_traits.rs index 8e0a68d..d0018b4 100644 --- a/src/num_traits.rs +++ b/src/num_traits.rs @@ -15,18 +15,95 @@ pub trait DetectGimbalLock: GimbalLockZenithNadir { fn close_to_zenith_or_nadir(&self, tolerance: T) -> bool; } -pub trait ArcTan { +pub trait IsNaN { + /// Determines whether this value represents "not a number" (NaN). + fn is_nan(&self) -> bool; +} + +pub trait NormalizeAngle { + type Output; + + /// Normalizes the angle into the range -π to π. + fn normalize_angle(self) -> Self::Output; +} + +pub trait ArcTan { type Output; fn atan2(self, rhs: T) -> Self::Output; } -pub trait ArcSin { +pub trait ArcSin { type Output; fn arcsin(self) -> Self::Output; } +pub trait Abs { + type Output; + + fn abs(self) -> Self::Output; +} + +impl IsNaN for f32 { + #[inline(always)] + fn is_nan(&self) -> bool { + (*self).is_nan() + } +} + +impl IsNaN for f64 { + #[inline(always)] + fn is_nan(&self) -> bool { + (*self).is_nan() + } +} + +#[cfg_attr(docsrs, doc(cfg(feature = "std")))] +#[cfg(feature = "std")] +impl NormalizeAngle for f32 { + type Output = f32; + + fn normalize_angle(self) -> Self::Output { + let mut normalized = self; + while normalized > std::f32::consts::PI { + normalized -= std::f32::consts::TAU; + } + while normalized < -std::f32::consts::PI { + normalized += std::f32::consts::TAU; + } + normalized + } +} + +#[cfg_attr(docsrs, doc(cfg(feature = "std")))] +#[cfg(feature = "std")] +impl NormalizeAngle for f64 { + type Output = f64; + + fn normalize_angle(self) -> Self::Output { + let mut normalized = self; + while normalized > std::f64::consts::PI { + normalized -= std::f64::consts::TAU; + } + while normalized < -std::f64::consts::PI { + normalized += std::f64::consts::TAU; + } + normalized + } +} + +#[cfg_attr(docsrs, doc(cfg(feature = "std")))] +#[cfg(feature = "std")] +impl Abs for f32 { + type Output = f32; + + #[inline(always)] + fn abs(self) -> Self::Output { + f32::abs(self) + } +} + #[cfg_attr(docsrs, doc(cfg(feature = "std")))] #[cfg(feature = "std")] impl ArcTan for f32 { diff --git a/src/test_estimator.rs b/src/test_estimator.rs new file mode 100644 index 0000000..94154f5 --- /dev/null +++ b/src/test_estimator.rs @@ -0,0 +1,966 @@ +use crate::vector3::Vector3; +use crate::{ + AccelerometerNoise, AccelerometerReading, ArcSin, ArcTan, DetectGimbalLock, EulerAngles, + GyroscopeBias, GyroscopeNoise, GyroscopeReading, IsNaN, MagnetometerNoise, MagnetometerReading, + NormalizeAngle, +}; +use core::ops::Neg; +use minikalman::buffers::types::*; +use minikalman::matrix::MatrixDataType; +use minikalman::prelude::*; +use minikalman::regular::{ + Control, ControlBuilder, RegularKalman, RegularKalmanBuilder, RegularObservation, + RegularObservationBuilder, +}; + +const STATES: usize = 6; // roll rate, pitch rate, yaw rate, as well as gyro bias (drift) terms +const CONTROLS: usize = 3; // roll rate, pitch rate, yaw rate +const OBSERVATIONS: usize = 3; // roll, pitch, yaw + +/// A MARG (Magnetic, Angular Rate, and Gravity) orientation estimator with generic type `T`. +pub struct OwnedOrientationEstimator { + filter: OwnedKalmanFilter, + control: OwnedControlInput, + measurement: OwnedObservation, + /// The angular tolerance, in radians, to detect a Gimbal Lock. + gimbal_lock_tolerance: T, +} + +impl OwnedOrientationEstimator { + /// Initializes a new instance of the [`OwnedOrientationEstimator`] struct. + /// + /// ## Arguments + /// * `gimbal_lock_tolerance` - The angular tolerance, in radians, to detect a Gimbal Lock. + /// * `accelerometer_noise` - The accelerometer noise values (sigma-squared) for each axis. + /// * `gyroscope_noise` - The gyroscope noise values (sigma-squared) for each axis. + /// * `magnetometer_noise` - The magnetometer noise values (sigma-squared) for each axis. + /// * `epsilon` - A small bias term to avoid divisions by zero. Set to e.g. `1e-6`. + pub fn new( + gimbal_lock_tolerance: T, + accelerometer_noise: AccelerometerNoise, + gyroscope_noise: GyroscopeNoise, + gyroscope_bias: GyroscopeBias, + magnetometer_noise: MagnetometerNoise, + epsilon: T, + ) -> Self + where + T: MatrixDataType + Default, + { + let filter = Self::build_filter(&gyroscope_noise, &gyroscope_bias, epsilon); + let control = Self::build_control(&gyroscope_noise, epsilon); + let measurement = Self::build_measurement(&accelerometer_noise, &magnetometer_noise); + + Self { + filter, + control, + measurement, + gimbal_lock_tolerance, + } + } +} + +impl OwnedOrientationEstimator { + /// Obtains the current estimates of the roll, pitch and yaw angles. + pub fn estimated_angles(&self) -> EulerAngles + where + T: Copy, + { + EulerAngles::new(self.roll(), self.pitch(), self.yaw()) + } + + /// Obtains the current estimate of the roll angle φ (phi), in radians. + /// + /// The roll angle is defined as the amount rotation around the y-axis (forward). + pub fn roll(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(0) + } + + /// Obtains the current estimation variance (uncertainty) of the roll angle φ (phi), in radians². + /// + /// ## Interpretation + /// - Low Variance: Indicates high certainty in the estimate. The state estimate is + /// considered to be precise, as it doesn't vary much from the mean. + /// - High Variance: Indicates high uncertainty in the estimate. The state estimate is + /// considered to be less precise, as it has a wide spread around the mean. + pub fn roll_variance(&self) -> T + where + T: Copy, + { + self.filter.estimate_covariance().get_at(0, 0) + } + + /// Obtains the current estimate of the bias term of the roll rate, in radians. + pub fn roll_rate_bias(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(3) + } + + /// Obtains the current estimate of the pitch angle θ (theta), in radians. + /// + /// The pitch angle is defined as the amount rotation around the x-axis (right). + pub fn pitch(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(1) + } + + /// Obtains the current estimation variance (uncertainty) of the pitch angle θ (theta), in radians². + /// + /// ## Interpretation + /// - Low Variance: Indicates high certainty in the estimate. The state estimate is + /// considered to be precise, as it doesn't vary much from the mean. + /// - High Variance: Indicates high uncertainty in the estimate. The state estimate is + /// considered to be less precise, as it has a wide spread around the mean. + pub fn pitch_variance(&self) -> T + where + T: Copy, + { + self.filter.estimate_covariance().get_at(1, 1) + } + + /// Obtains the current estimate of the bias term of the pitch rate, in radians. + pub fn pitch_rate_bias(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(4) + } + + /// Obtains the current estimate of the yaw angle ψ (psi), in radians. + /// + /// The yaw angle is defined as the amount rotation around the z-axis (up). + pub fn yaw(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(2) + } + + /// Obtains the current estimation variance (uncertainty) of the yaw angle ψ (psi), in radians². + /// + /// ## Interpretation + /// - Low Variance: Indicates high certainty in the estimate. The state estimate is + /// considered to be precise, as it doesn't vary much from the mean. + /// - High Variance: Indicates high uncertainty in the estimate. The state estimate is + /// considered to be less precise, as it has a wide spread around the mean. + pub fn yaw_variance(&self) -> T + where + T: Copy, + { + self.filter.estimate_covariance().get_at(2, 2) + } + + /// Obtains the current estimate of the bias term of the yaw rate, in radians. + pub fn yaw_rate_bias(&self) -> T + where + T: Copy, + { + self.filter.state_vector().get_row(5) + } +} + +impl OwnedOrientationEstimator { + /// Performs a prediction step to obtain new orientation estimates. + /// + /// ## Arguments + /// * `delta_t` - The time step for the prediction. + /// * `angular_rates` - The angular rates measured by the gyroscope. + pub fn predict(&mut self, delta_t: T, angular_rates: &GyroscopeReading) + where + T: MatrixDataType + Default + NormalizeAngle + IsNaN, + { + // Update the Kalman filter components. + self.update_state_transition_matrix(delta_t, angular_rates); + self.update_control_input(delta_t, angular_rates); + + // Perform a regular Kalman Filter prediction step. + self.filter.predict(); + self.panic_if_nan(); + + // Apply the Gyro control input. + self.filter.control(&mut self.control); + self.panic_if_nan(); + + // Normalize the state estimates. + self.normalize_angles(); + } + + /// Performs a correction step using accelerometer and magnetometer readings. + /// + /// ## Arguments + /// * `accelerometer` - The accelerometer reading. + /// * `magnetometer` - The magnetometer reading. + pub fn correct( + &mut self, + accelerometer: &AccelerometerReading, + magnetometer: &MagnetometerReading, + ) where + T: MatrixDataType + + ArcSin + + ArcTan + + NormalizeAngle + + DetectGimbalLock + + core::fmt::Debug + + IsNaN, + { + // Normalize the vectors. + let a = Vector3::from(accelerometer).normalized(); + let m = Vector3::from(magnetometer).normalized(); + + // Obtain the Euler Angles and apply them to the measurement. + let angles = self.build_triad(a, m); + self.measurement.measurement_vector_mut().apply(|vec| { + vec.set_row(0, angles.roll_phi); + vec.set_row(1, angles.pitch_theta); + vec.set_row(2, angles.yaw_psi); + }); + + // Update the measurement noise. + self.update_measurement_noise(a, m); + + // Perform the update step. + self.filter.correct(&mut self.measurement); + self.panic_if_nan(); + + // Normalize the state estimates. + self.normalize_angles(); + } + + // Normalizes the state estimates. + fn normalize_angles(&mut self) + where + T: Copy + NormalizeAngle, + { + self.filter.state_vector_mut().apply(|vec| { + vec.set_row(0, vec.get_row(0).normalize_angle()); + vec.set_row(1, vec.get_row(1).normalize_angle()); + vec.set_row(2, vec.get_row(2).normalize_angle()); + }); + } + + #[allow(unused)] + fn panic_if_nan(&self) + where + T: Copy + IsNaN, + { + #[cfg(debug_assertions)] + self.filter.state_vector().inspect(|vec| { + if vec.get_row(0).is_nan() || vec.get_row(1).is_nan() || vec.get_row(2).is_nan() { + panic!("NaN angle detected in state estimate") + } + }); + } + + /// Computes the Jacobian matrices and measurement noise matrix for orientation estimation + /// + /// This is done by propagating the sensor noise through the TRIAD method + /// The final measurement noise covariance matrix R is computed using J_a, R_a, J_m, and R_m + /// R represents the combined effect of accelerometer and magnetometer noise on the orientation estimates, + /// where J_a and J_m are the Jacobians of the respective measurement noise. + /// + /// ## Arguments + /// * `accelerometer` - The normalized accelerometer vector. + /// * `magnetometer` - The normalized magnetometer vector. + #[allow(unused)] + fn update_measurement_noise(&mut self, accelerometer: Vector3, magnetometer: Vector3) + where + T: MatrixDataType + core::fmt::Debug, + { + /* + // Easy access to accelerometer noise. + let sa11 = self.accelerometer_noise.x; // sigma_a_xx + let sa22 = self.accelerometer_noise.y; // sigma_a_yy + let sa33 = self.accelerometer_noise.z; // sigma_a_zz + + // Easy access to magnetometer noise. + let sm11 = self.magnetometer_noise.x; // sigma_m_xx + let sm22 = self.magnetometer_noise.y; // sigma_m_yy + let sm33 = self.magnetometer_noise.z; // sigma_m_zz + + // Helper "constants". + let two = T::one() + T::one(); + let epsilon = self.epsilon; + + // Easy access to accelerometer components. + let ax = accelerometer.x; + let ay = accelerometer.y; + let az = accelerometer.z; + + // Easy access to magnetometer components. + let mx = magnetometer.x; + let my = magnetometer.y; + let mz = magnetometer.z; + + // Common squares of the accelerometer components. + let ax2 = accelerometer.x * accelerometer.x; + let ay2 = accelerometer.y * accelerometer.y; + let az2 = accelerometer.z * accelerometer.z; + + // Common squares of the magnetometer components. + let mx2 = magnetometer.x * magnetometer.x; + let my2 = magnetometer.y * magnetometer.y; + let mz2 = magnetometer.z * magnetometer.z; + + // Noise matrix. + let r = self.measurement.measurement_noise_covariance_mut(); + + // Common denominator of the matrix components. + #[rustfmt::skip] + let denom = + ax2 * az2 * mx2 + + ax2 * my2 + + two * ax2 * ay * az2 * mx * my + - two * ax * ay * mx * my + + two * ax * az2 * az * mx * mz + - two * ax * az * mx * mz + + ay2 * az2 * my2 + + ay2 * mx2 + + two * ay * az2 * az * my * mz + - two * ay * az * my * mz + + az2 * az2 * mz2 + - two * az2 * mz2 + + mz2; + + let one_minus_az2 = T::one() - az2; + let one_minus_az2_sqrt = one_minus_az2.square_root(); + + // Row 1, column 1. + let r11_nom = sa11 * sq(ay * az * mx2 + ay * az * my2 + az2 * my * mz - my * mz) + + sa22 * sq(ax * az * mx2 + ax * az * my2 + az2 * mx * mz - mx * mz) + + sa33 + * sq( + ax2 * mx * my - ax * my * mx2 + ax * ay * my2 + two * ax * az * my * mz + - ay2 * mx * my + - two * ay * az * mx * mz, + ) + + sm11 * sq(ax2 * az * my + ay2 * az * my + ay * az2 * mz - ay * mz) + + sm22 * sq(ax2 * az * mx + ax * az2 * mz - ax * mz + ay2 * az * mx) + + sm33 * sq(ax * az2 * my - ax * my - ay * az2 * mx + ay * mx); + let r11 = r11_nom / (sq(denom) + epsilon); + r.set_at(0, 0, r11); + + // Row 1, column 2 and row 2, column 1. + let r12_nom = sa33 + * (-ax2 * mx * my + ax * ay * mx2 - ax * ay * my2 - two * ax * az * my * mz + + ay2 * mx * my + + two * ay * az * mx * mz); + let r12 = r12_nom / (one_minus_az2_sqrt * denom + epsilon); + r.set_symmetric(0, 1, r12); + + // Row 1, column 3 and row 3, column 1. + let r13_nom = ax * sa22 * (ax * az * mx2 + ax * az * my2 + az2 * mx * mz - mx * mz) + + ay * sa11 * (ay * az * mx2 + ay * az * my2 + az2 * my * mz - my * mz); + let r13 = r13_nom / ((ax2 + ay2) * denom + epsilon); + r.set_symmetric(0, 2, r13); + + // Row 2, column 2. + let r22 = -sa33 / (ax2 - T::one() + epsilon); + r.set_at(1, 1, r22); + + // Row 2, column 3 and row 3, column 2. + r.set_symmetric(1, 2, T::zero()); + + // Row 3, column 3. + let r33 = (ax2 * sa22 + ay2 * sa11) / (sq(ax2 + ay2) + epsilon); + r.set_at(2, 2, r33); + */ + } + + /// Constructs the state transition matrix based on the angular rates. + fn update_state_transition_matrix(&mut self, delta_t: T, angular_rates: &GyroscopeReading) + where + T: MatrixDataType + Default, + { + let rates = *angular_rates * delta_t; + let x_bias = self.roll_rate_bias() * delta_t; + let y_bias = self.pitch_rate_bias() * delta_t; + let z_bias = self.yaw_rate_bias() * delta_t; + + self.filter.state_transition_mut().apply(|mat| { + mat.set_at(0, 0, T::one()); + mat.set_at(0, 1, -(rates.omega_z - z_bias)); + mat.set_at(0, 2, rates.omega_y - y_bias); + mat.set_at(0, 3, -delta_t); + mat.set_at(0, 4, T::zero()); + mat.set_at(0, 5, T::zero()); + + mat.set_at(1, 0, rates.omega_z - z_bias); + mat.set_at(1, 1, T::one()); + mat.set_at(1, 2, -(rates.omega_x - x_bias)); + mat.set_at(1, 3, T::zero()); + mat.set_at(1, 4, -delta_t); + mat.set_at(1, 5, T::zero()); + + mat.set_at(2, 0, -(rates.omega_y - y_bias)); + mat.set_at(2, 1, rates.omega_x - x_bias); + mat.set_at(2, 2, T::one()); + mat.set_at(2, 3, T::zero()); + mat.set_at(2, 4, T::zero()); + mat.set_at(2, 5, -delta_t); + + mat.set_at(3, 3, T::one()); + mat.set_at(4, 4, T::one()); + mat.set_at(5, 5, T::one()); + }); + } + + /// Constructs the state transition matrix based on the angular rates. + fn update_control_input(&mut self, delta_t: T, angular_rates: &GyroscopeReading) + where + T: MatrixDataType + Default, + { + self.control.control_matrix_mut().apply(|mat| { + mat.set_at(0, 0, delta_t); + mat.set_at(1, 1, delta_t); + mat.set_at(2, 2, delta_t); + }); + + let rates = *angular_rates * delta_t; + self.control.control_vector_mut().apply(|vec| { + vec.set_row(0, rates.omega_x); + vec.set_row(1, rates.omega_y); + vec.set_row(2, rates.omega_z); + }); + } + + /// Builds the TRIAD rotation matrix and obtains the Euler angles from it. + /// + /// ## Arguments + /// * `a` - The normalized accelerometer vector. + /// * `m` - The normalized magnetometer vector. + fn build_triad(&self, a: Vector3, m: Vector3) -> EulerAngles + where + T: MatrixDataType + + ArcTan + + ArcSin + + Neg + + DetectGimbalLock + + NormalizeAngle, + { + // Define base vectors (i.e., TRIAD). + let z = -a; // up + let _x = m; // forward + let y = m.cross(z).normalized(); // left + let x = y.cross(z).normalized(); // forward + + // TRIAD rotation matrix: [b1, b2, b3], stacked columns + + // Derive Euler angles from TRIAD rotation matrix (Trait-Bryan XYZ order). + let theta = (-ArcSin::arcsin(z.x)).normalize_angle(); // pitch + + // Handle Gimbal lock situations. + #[allow(unreachable_code)] + if theta.close_to_zenith_or_nadir(self.gimbal_lock_tolerance) { + todo!("test this"); + let psi = ArcTan::atan2(-x.y, y.y); // yaw + let phi = T::zero(); // roll + EulerAngles::new(phi.normalize_angle(), theta, psi.normalize_angle()) + } else { + let phi = ArcTan::atan2(z.y, z.z); // roll + let psi = ArcTan::atan2(y.x, x.x); // yaw + EulerAngles::new(phi.normalize_angle(), theta, psi.normalize_angle()) + } + } +} + +impl OwnedOrientationEstimator { + /// Builds the Kalman filter used for prediction. + fn build_filter( + gyroscope_noise: &GyroscopeNoise, + gyroscope_bias: &GyroscopeBias, + process_noise_value: T, + ) -> OwnedKalmanFilter + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // State vector. + let mut state_vec = + StateVectorBuffer::::new(MatrixData::new_array::( + [zero; STATES], + )); + state_vec.set_row(3, gyroscope_bias.omega_x); + state_vec.set_row(4, gyroscope_bias.omega_y); + state_vec.set_row(5, gyroscope_bias.omega_z); + + // State transition matrix. + let state_transition = + StateTransitionMatrixMutBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + // Estimate covariance matrix. + let mut estimate_covariance = + EstimateCovarianceMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + estimate_covariance.apply(|mat| { + mat.set_at(0, 0, gyroscope_noise.x); + // mat.set_at(0, 1, gyroscope_noise.z * gyroscope_noise.x); + // mat.set_at(0, 2, gyroscope_noise.y * gyroscope_noise.x); + + // mat.set_at(1, 0, gyroscope_noise.z * gyroscope_noise.y); + mat.set_at(1, 1, gyroscope_noise.y); + // mat.set_at(1, 2, gyroscope_noise.x * gyroscope_noise.y); + + // mat.set_at(2, 0, gyroscope_noise.y * gyroscope_noise.z); + // mat.set_at(2, 1, gyroscope_noise.x * gyroscope_noise.z); + mat.set_at(2, 2, gyroscope_noise.z); + + mat.set_at(3, 3, gyroscope_noise.x * gyroscope_noise.x); + mat.set_at(4, 4, gyroscope_noise.y * gyroscope_noise.y); + mat.set_at(5, 5, gyroscope_noise.z * gyroscope_noise.z); + }); + + // Process noise matrix. + let mut process_noise = DirectProcessNoiseCovarianceMatrixMutBuffer::::new( + MatrixData::new_array::( + [zero; { STATES * STATES }], + ), + ); + process_noise.make_scalar(process_noise_value); + + // Predicted state vector. + let mut predicted_state = + PredictedStateEstimateVectorBuffer::::new(MatrixData::new_array::< + STATES, + 1, + STATES, + T, + >([zero; STATES])); + predicted_state.set_all(T::zero()); + + // Temporary estimate covariance matrix. + let temp_state_matrix = + TemporaryStateMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + RegularKalmanBuilder::new::( + state_transition, + state_vec, + estimate_covariance, + process_noise, + predicted_state, + temp_state_matrix, + ) + } + + /// Builds the Kalman filter control input. + fn build_control( + _gyroscope_noise: &GyroscopeNoise, + process_noise_value: T, + ) -> OwnedControlInput + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // Control vector. + let control_vector = ControlVectorBuffer::::new(MatrixData::new_array::< + CONTROLS, + 1, + CONTROLS, + T, + >([zero; CONTROLS])); + + // Control matrix. + let mut control_matrix = + ControlMatrixMutBuffer::::new(MatrixData::new_array::< + STATES, + CONTROLS, + { STATES * CONTROLS }, + T, + >( + [zero; STATES * CONTROLS] + )); + control_matrix.apply(|mat| { + mat.set_at(0, 0, T::one()); + mat.set_at(1, 1, T::one()); + mat.set_at(2, 2, T::one()); + }); + + // Process noise matrix. + let mut process_noise = ControlProcessNoiseCovarianceMatrixMutBuffer::::new( + MatrixData::new_array::( + [zero; CONTROLS * CONTROLS], + ), + ); + process_noise.make_scalar(process_noise_value); + + // Temporary matrix. + let temp = TemporaryBQMatrixBuffer::::new(MatrixData::new_array::< + STATES, + CONTROLS, + { STATES * CONTROLS }, + T, + >( + [zero; STATES * CONTROLS], + )); + + ControlBuilder::new::( + control_matrix, + control_vector, + process_noise, + temp, + ) + } + + /// Builds the Kalman filter observation used for the prediction. + fn build_measurement( + accelerometer_noise: &AccelerometerNoise, + magnetometer_noise: &MagnetometerNoise, + ) -> OwnedObservation + where + T: MatrixDataType + Default, + { + let zero = T::default(); + + // Measurement vector + let measurement = + MeasurementVectorBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + 1, + OBSERVATIONS, + T, + >([zero; OBSERVATIONS])); + + // Observation matrix + let mut observation_matrix = + ObservationMatrixMutBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + STATES, + { OBSERVATIONS * STATES }, + T, + >( + [zero; { OBSERVATIONS * STATES }], + )); + observation_matrix.apply(|mat| { + mat.set_at(0, 0, T::one()); + mat.set_at(1, 1, T::one()); + mat.set_at(2, 2, T::one()); + }); + + // Measurement noise covariance + let mut noise_covariance = + MeasurementNoiseCovarianceMatrixBuffer::::new( + MatrixData::new_array::< + OBSERVATIONS, + OBSERVATIONS, + { OBSERVATIONS * OBSERVATIONS }, + T, + >([zero; { OBSERVATIONS * OBSERVATIONS }]), + ); + + let noise_covariance_value = accelerometer_noise.x * magnetometer_noise.x + + accelerometer_noise.y * magnetometer_noise.y + + accelerometer_noise.z * magnetometer_noise.z; + noise_covariance.make_scalar(noise_covariance_value); + + // Innovation vector + let innovation_vector = + InnovationVectorBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + 1, + OBSERVATIONS, + T, + >([zero; OBSERVATIONS])); + + // Innovation covariance matrix + let innovation_covariance = + InnovationCovarianceMatrixBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + OBSERVATIONS, + { OBSERVATIONS * OBSERVATIONS }, + T, + >( + [zero; { OBSERVATIONS * OBSERVATIONS }], + )); + + // Kalman Gain matrix + let kalman_gain = + KalmanGainMatrixBuffer::::new(MatrixData::new_array::< + STATES, + OBSERVATIONS, + { STATES * OBSERVATIONS }, + T, + >( + [zero; { STATES * OBSERVATIONS }], + )); + + // Temporary residual covariance inverted matrix + let temp_sinv = TemporaryResidualCovarianceInvertedMatrixBuffer::::new( + MatrixData::new_array::( + [zero; { OBSERVATIONS * OBSERVATIONS }], + ), + ); + + // Temporary H×P matrix + let temp_hp = + TemporaryHPMatrixBuffer::::new(MatrixData::new_array::< + OBSERVATIONS, + STATES, + { OBSERVATIONS * STATES }, + T, + >( + [zero; { OBSERVATIONS * STATES }], + )); + + // Temporary P×Hᵀ matrix + let temp_pht = + TemporaryPHTMatrixBuffer::::new(MatrixData::new_array::< + STATES, + OBSERVATIONS, + { STATES * OBSERVATIONS }, + T, + >( + [zero; { STATES * OBSERVATIONS }], + )); + + // Temporary K×(H×P) matrix + let temp_khp = TemporaryKHPMatrixBuffer::::new(MatrixData::new_array::< + STATES, + STATES, + { STATES * STATES }, + T, + >( + [zero; { STATES * STATES }] + )); + + RegularObservationBuilder::new::( + observation_matrix, + measurement, + noise_covariance, + innovation_vector, + innovation_covariance, + kalman_gain, + temp_sinv, + temp_hp, + temp_pht, + temp_khp, + ) + } +} + +type OwnedKalmanFilter = RegularKalman< + STATES, + T, + StateTransitionMatrixMutBuffer< + STATES, + T, + MatrixDataArray, + >, + StateVectorBuffer>, + EstimateCovarianceMatrixBuffer< + STATES, + T, + MatrixDataArray, + >, + DirectProcessNoiseCovarianceMatrixMutBuffer< + STATES, + T, + MatrixDataArray, + >, + PredictedStateEstimateVectorBuffer>, + TemporaryStateMatrixBuffer>, +>; + +type OwnedControlInput = Control< + STATES, + CONTROLS, + T, + ControlMatrixMutBuffer>, + ControlVectorBuffer>, + ControlProcessNoiseCovarianceMatrixMutBuffer< + CONTROLS, + T, + MatrixDataArray, + >, + TemporaryBQMatrixBuffer< + STATES, + CONTROLS, + T, + MatrixDataArray, + >, +>; + +type OwnedObservation = RegularObservation< + STATES, + OBSERVATIONS, + T, + ObservationMatrixMutBuffer< + OBSERVATIONS, + STATES, + T, + MatrixDataArray, + >, + MeasurementVectorBuffer>, + MeasurementNoiseCovarianceMatrixBuffer< + OBSERVATIONS, + T, + MatrixDataArray, + >, + InnovationVectorBuffer>, + InnovationCovarianceMatrixBuffer< + OBSERVATIONS, + T, + MatrixDataArray, + >, + KalmanGainMatrixBuffer< + STATES, + OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryResidualCovarianceInvertedMatrixBuffer< + OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryHPMatrixBuffer< + OBSERVATIONS, + STATES, + T, + MatrixDataArray, + >, + TemporaryPHTMatrixBuffer< + STATES, + OBSERVATIONS, + T, + MatrixDataArray, + >, + TemporaryKHPMatrixBuffer>, +>; + +#[cfg(test)] +#[cfg(feature = "std")] +mod tests { + use super::*; + use crate::accelerometer_reading::AccelerometerReading; + + trait Round { + /// Rounds to two decimal places. + fn round2(self) -> T; + + /// Rounds to seven decimal places. + fn round7(self) -> T; + } + + impl Round for f32 { + fn round2(self) -> f32 { + (self * 100.0).round() * 0.01 + } + + fn round7(self) -> f32 { + (self * 1000000.0).round() * 0.000001 + } + } + + #[test] + fn test_state_transition_matrix() { + let gimbal_lock_tolerance = 0.01; + let accelerometer_noise = AccelerometerNoise::new(0.01, 0.02, 0.03); + let gyroscope_noise = GyroscopeNoise::new(0.04, 0.05, 0.06); + let gyroscope_bias = GyroscopeBias::default(); + let magnetometer_noise = MagnetometerNoise::new(0.07, 0.08, 0.09); + let epsilon = 1e-6; + + let mut estimator: OwnedOrientationEstimator = OwnedOrientationEstimator::new( + gimbal_lock_tolerance, + accelerometer_noise, + gyroscope_noise, + gyroscope_bias, + magnetometer_noise, + epsilon, + ); + + // Set up example error covariances (sigma phi/theta/psi). + estimator.filter.estimate_covariance_mut().make_scalar(0.1); + + // Set up example process noise. + // TODO: Pass as argument + estimator + .filter + .direct_process_noise_mut() + .make_scalar(0.01); + + // Set up example measurement noise. + // TODO: Pass as argument + estimator + .measurement + .measurement_noise_covariance_mut() + .make_scalar(0.01); + + let delta_t: f32 = 0.1; + let angular_rates = GyroscopeReading::new(0.01, 0.02, -0.01); + estimator.predict(delta_t, &angular_rates); + + estimator.filter.state_transition().inspect(|mat| { + assert_eq!(mat.get_at(0, 0), 1.0); + assert_eq!(mat.get_at(0, 1), 0.001); + assert_eq!(mat.get_at(0, 2), 0.002); + + assert_eq!(mat.get_at(1, 0), -0.001); + assert_eq!(mat.get_at(1, 1), 1.0); + assert_eq!(mat.get_at(1, 2), -0.001); + + assert_eq!(mat.get_at(2, 0), -0.002); + assert_eq!(mat.get_at(2, 1), 0.001); + assert_eq!(mat.get_at(2, 2), 1.0); + }); + + estimator.filter.state_vector().inspect(|vec| { + assert_eq!(vec.get_row(0), 0.001); + assert_eq!(vec.get_row(1), 0.002); + assert_eq!(vec.get_row(2), -0.001); + }); + + estimator.filter.estimate_covariance().inspect(|mat| { + assert_eq!(mat.get_at(0, 0).round2(), 0.11); + assert_eq!(mat.get_at(0, 1).round2(), -0.0); + assert_eq!(mat.get_at(0, 2).round2(), 0.0); + + assert_eq!(mat.get_at(1, 0).round2(), -0.0); + assert_eq!(mat.get_at(1, 1).round2(), 0.11); + assert_eq!(mat.get_at(1, 2).round2(), 0.0); + + assert_eq!(mat.get_at(2, 0).round2(), 0.0); + assert_eq!(mat.get_at(2, 1).round2(), 0.0); + assert_eq!(mat.get_at(2, 2).round2(), 0.11); + }); + + // Incorporate accelerometer vector. + let accelerometer = AccelerometerReading::new(0.0, 0.0, -9.81); + let magnetometer = MagnetometerReading::new(0.2, 0.0, 0.4); + + estimator.correct(&accelerometer, &magnetometer); + + estimator.filter.state_vector().inspect(|vec| { + assert_eq!(vec.get_row(0).round7(), 0.0); // replace with approximate tests + assert_eq!(vec.get_row(1).round2(), 1.23); // replace with approximate tests + assert_eq!(vec.get_row(2).round2(), -1.5699999); // replace with approximate tests + }) + } +} diff --git a/src/vector3.rs b/src/vector3.rs index dbeedd7..1194b67 100644 --- a/src/vector3.rs +++ b/src/vector3.rs @@ -1,11 +1,13 @@ use crate::accelerometer_reading::AccelerometerReading; -use crate::impl_standard_traits; use crate::magnetometer_reading::MagnetometerReading; use core::borrow::Borrow; use core::fmt::{Debug, Formatter}; -use core::ops::{Add, Mul, Sub}; +use core::ops::{Add, Mul, Neg, Sub}; use minikalman::matrix::MatrixDataType; +use uniform_array_derive::UniformArray; +/// A three-dimensional vector. +#[derive(UniformArray, Copy)] #[cfg_attr(test, ensure_uniform_type::ensure_uniform_type)] #[repr(C)] pub struct Vector3 { @@ -158,6 +160,23 @@ impl From> for Vector3 { } } +/// Implements the unary negation. +impl Neg for Vector3 +where + T: Neg, +{ + type Output = Vector3; + + #[inline] + fn neg(self) -> Self::Output { + Self { + x: -self.x, + y: -self.y, + z: -self.z, + } + } +} + /// Implements the vector dot product. impl Mul> for Vector3 where @@ -220,36 +239,12 @@ where } } -#[cfg(not(feature = "unsafe"))] -impl core::ops::Index for Vector3 { - type Output = T; - - #[inline(always)] - fn index(&self, index: usize) -> &Self::Output { - match index { - 0 => &self.x, - 1 => &self.y, - 2 => &self.z, - _ => panic!("Index out of bounds"), - } - } -} - -#[cfg(not(feature = "unsafe"))] -impl core::ops::IndexMut for Vector3 { - #[inline(always)] - fn index_mut(&mut self, index: usize) -> &mut Self::Output { - match index { - 0 => &mut self.x, - 1 => &mut self.y, - 2 => &mut self.z, - _ => panic!("Index out of bounds"), - } +impl From> for (T, T, T) { + fn from(value: Vector3) -> Self { + (value.x, value.y, value.z) } } -impl_standard_traits!(Vector3, T); - #[cfg(test)] mod test { use super::*; diff --git a/tests/data b/tests/data new file mode 160000 index 0000000..de4a6e4 --- /dev/null +++ b/tests/data @@ -0,0 +1 @@ +Subproject commit de4a6e4d462603d2f10d4dfe4fdc070b69f95967