From 5daac77a37805e26f2946cb044cad52338c378c0 Mon Sep 17 00:00:00 2001 From: Pima Date: Wed, 23 Sep 2026 18:31:51 +0200 Subject: [PATCH] Added hemisphere continuity enforcement in the madgwick algorithm --- Core/Src/MadgwickAHRS.c | 17 +++++++++++++++++ Debug/Core/Src/MadgwickAHRS.cyclo | 6 +++--- Debug/Core/Src/MadgwickAHRS.o | Bin 64400 -> 64928 bytes Debug/Core/Src/MadgwickAHRS.su | 4 ++-- 4 files changed, 22 insertions(+), 5 deletions(-) diff --git a/Core/Src/MadgwickAHRS.c b/Core/Src/MadgwickAHRS.c index 135cc2a..a69e305 100644 --- a/Core/Src/MadgwickAHRS.c +++ b/Core/Src/MadgwickAHRS.c @@ -148,6 +148,14 @@ void MadgwickAHRS_update(MadgwickAHRS_Filter* filter, float gx, float gy, float q[2] *= recipNorm; q[3] *= recipNorm; + // enforce hemisphere continuity (because of the positive and negative doubling of output) + float dot = filter->q[0] * q[0] + filter->q[1] * q[1] + filter->q[2] * q[2] + filter->q[3] * q[3]; + if (dot < 0.0f){ + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } // Update filter state filter->q[0] = q[0]; filter->q[1] = q[1]; @@ -226,6 +234,15 @@ void MadgwickAHRS_update_IMU(MadgwickAHRS_Filter* filter, float gx, float gy, fl q[2] *= recipNorm; q[3] *= recipNorm; + // enforce hemisphere continuity (because of the positive and negative doubling of output) + float dot = filter->q[0] * q[0] + filter->q[1] * q[1] + filter->q[2] * q[2] + filter->q[3] * q[3]; + if (dot < 0.0f){ + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + q[3] = -q[3]; + } + // Update filter state filter->q[0] = q[0]; filter->q[1] = q[1]; diff --git a/Debug/Core/Src/MadgwickAHRS.cyclo b/Debug/Core/Src/MadgwickAHRS.cyclo index ffa83a2..5b47ebe 100644 --- a/Debug/Core/Src/MadgwickAHRS.cyclo +++ b/Debug/Core/Src/MadgwickAHRS.cyclo @@ -1,4 +1,4 @@ ../Core/Src/MadgwickAHRS.c:42:6:MadgwickAHRS_init 1 -../Core/Src/MadgwickAHRS.c:55:6:MadgwickAHRS_update 7 -../Core/Src/MadgwickAHRS.c:161:6:MadgwickAHRS_update_IMU 4 -../Core/Src/MadgwickAHRS.c:240:7:invSqrt 1 +../Core/Src/MadgwickAHRS.c:55:6:MadgwickAHRS_update 8 +../Core/Src/MadgwickAHRS.c:169:6:MadgwickAHRS_update_IMU 5 +../Core/Src/MadgwickAHRS.c:257:7:invSqrt 1 diff --git a/Debug/Core/Src/MadgwickAHRS.o b/Debug/Core/Src/MadgwickAHRS.o index 18394457631d2e0e28368af85f99eceb111a9a7a..ae23e5de8502259d6d955bc495270f943457fe58 100644 GIT binary patch delta 4827 zcmcIldvH`|75~0_bN9V>@7+f>n}mcUu%QXr5FSk$QXo>11g2ssg3wB|&^(}17$^-l z9hR{uom7!{YC%9-#}1T7d8O74kG4A12*uGEt6?Bm9icDSOcKJQiKCu#?{}9ij^khM z%--|;e&_L>?|kQ;yZx5@@~FIhtkje`R{RDJoqVjLZCT4GK6kfFU(wNav84m9@_F~X z_Ev8yU>xgjHhNPoqqM&irPkJ)avG2K?=z}RUPtVJwSmxhkFg`A-eu_!mgGs6PSesW zES(}|fMVPg(*=LSm_A|Y$gs3`x3h6kOzwv~`J<$2JdK&{NJ&OBX4{jRg!|eY7hGiq z#{`!^E^wD`McS?>H8HL4NtNSTGi`fPWw_SG`W~YmpUxGM*ac%ozfi+{MwP}$cY=^OkwTz?+1-A7Ch;`KKFjI-@bWfET zx1m9^QXbPXZn7LL7-5t;98@Z~4i}X?o$fL5IZvl!I_hRSmb*3k2H~Ki5V`jp<@mhi z_&(QQt8+2D=m|rcdlnq`Er4&i=JEi%=c;y6E-~YA@cSD4)Ooq)uGnX=+&#}vTAK;L zFWfJ|Syv5g^SE#~%8u+_@+^c#uLfd@gOO zUER^PtI5FUg{I$DpPsY%^yZ(x0+B#bb;o0)t;KuKqd3KwSh(>=tnl~`MI|6b(Pj5x*V zIM(-M41HJTqeC7lGB+d~&2Supn2Q9ctHb6dRIySlDV5!7onrX5eoxX0o-aOHxp&1T zoEba+uGk@rmxGOK$F_$V6~QhN9Wt76$UE;K786psyWA=iy?qhPZ6C@o-WGjGOK}~_ zSO;OP7HXv?jG)GDLMkzx(#B!Ll$nh~h$*^_gNWV6XzPCtGJ3r{X-yd34#l7|i{Z}=KaPy?cygp{ z_acN|csEj1?puw^mH7A5LuOf7&K(ah6j>{uWK4Zz6=PDaT!y*;XxR&sTDt?y)&$NW z^9lZSTCiYGF=L!_Cq<)QJO25e`#EEwc2Y!0A$6??|7$)fB<`~Cdgn$a+hC2em1S|~ zQPXlYbNM}t?MJ{maZA3ZPr{=lpKU~P3a#a?ElH`lo$aruSaR29L>t-uc^3M|CaKwA z`xja0_9p2YXt~@%!-&wb)j|V^&~lxHu5FUnvI8zEkleK=$-j6nV+ZYQ<#owP?2w7% zu2V^QT@^cI7Etbb6VYULNVB56j%W%y6t>XOB!B)3jJ+nJemBVr;Taa1&x6YlJz$}^9M&Rgw9o@QG9A$Z3(b;OJXyJ(^^5Z#O0r1SI!xMC zTf)B2bpNbrGq8f6gl9uXe}lpOyYUJ50tMVp5wc#$FAMpokgJ5;DCAa4CfN@K@iQTh3Hi2=e;4v&l6dX61n!cpM0$mk zaDr1}SV&VA2su&6S|J;RTw=;3W6grtAmkPye<0+`LLL_Kl#pkIJTK&jLJkWlW1%Ub zppZpEP7-n^$%$z2pdg+Qay?1>Ck6hVkh{C5o!wFBDZ|^Mn<-bj|NO}e2OogQK#BCs z8K@nID?4!*cEU;|PMv|yfw(j-8;&EtC!4VYa0&VM&ww--SLWn1wgD;zOO#V%7^{J< zlObps)TH_6z(DmdlBFj3CX(yV!P&vMlve;lC|)=RxkGWs^g_nYd*C~_OZe}gZ78mU z$`~s)eV+SF=Y4PjxoOBPGdb5S;yl;m%68<+Om63G;+7(J0=XAVZpn})_4UC4Bp)ni zY<8O5atTI|>_T#?<$Yb}m!NSt?)Wuw+c~^ARKouPyN2V+q6)?qn?`Tm)cFVSDH`04x;Sc=?Lh6dy%Xb4QjQ62-%5DJ=PN$Ufv;l^?4 z_<%tNlR3^{foVl(XtC3QhEgU#Rq7Cmc1m@|I6&=ytq8iQLJ&-#^!x8UciGyVx%WHY z_dm{m&-u^U*GJT@t7_LMW#Je5st)kz$g0lt^46)VJJZ`*JMli4B`?{6KDC=n|UbC zEq)P-Wh_6p%pc0-yOrM~^QZFiYcjtpFV3#ZuG5aouS-WwPUFhpOyldp(6Iiu0a2?C zWP--i_@XUY1=Xie{cZ5FH7>OYc^Z8s`HhDb`cCZka(?~Ux}3c~TZ?hn^zF~qU>uR- zamGLflhvaDaGzr?s-`( z_Dyk9MRDU1p#|na4~dKJM#RqduMj@p0v;CcdFsVxpGOS)3&b8@qegWk%=++MVug3U z7R*z^Vu^nz;#UM>A{6+6Hz+;Bj`tV&aGvuP!jZ zkWNW|mbzEZ*`u~aaQ#P4T2WoxugConc+nXO8Y%IDUZLGOIT3u-N;M{q>EkC_doWQp zLBtqd)0s{*H?()Amo*!BZ*A_w`&9Gs`Xl$OKeGP!Z-{Nd%FNVNBW=rDJMrGudI;~+ zts|=jP?vSu?NgHAF6}XJtAQ+g0XP*AGgnO=$d-!_BjYApJsE|O3M!CG zT81T5^Y*?7=ZvLW2791u;?ZaeY1;k@{yab%f-l4mb`n(AJ#xUOo zjCDEhDEQg)Ql133b#|;?qPi5ylaUiABo@PvUjXs^0w_^_0d%z$<`sZrW;SmH{Okf+ z;1kRHaP!b(J}N9z4o9;R>^+@ox?H13vKvTuB>hK{BVEY$m@ePl@J|_g2U~nMum0!e zhAj1A6n=Ie(oWFo*qrVGD%R#_)U3S}Hwr4&{D1xAZPypTq|}x+w{E*z zT!)?7(zXYUhBYH?iDm=uWzBtfZ*3mN`&2V~YojPD%xFJ#GUjC5!SM5lUl*47yv0hP zcM+&hoGh%Y^|xdAFrEOtROWEFx6EMx$HR{@raiQVF(skaz^04Rq6ad;4j0@W3%(EK z0-g{JSh&B6G5(dClF_dN58r+#M{o!GDn_!>y_(-#z}VLwi#@5XW0&OkF}00dcJDR4 z+L>GPF}4GQIWQOBeoVnPJl6zm^05rYYWK?-rKN-Im_?b??u}sU*pB%YdmhYSI~G}N zeMVUc&lMKC{;aYRo^2Mp1_sZy78`n2eVo1Fp_Ng)U(fKlcQUp!SL@o_*e)U&7!POE zwG-K{fE9NTY!cfQve#@I@f2@P)Ry-bnWaG=O&R5ndOzpaW%}Jvdha^2+ZCo15mwF= zEI*DIbM#*M({5Mh3Wf?6BLqz;z@|u?M(Aa1zT}4_a-1dd3rQ@OSSPVTVxz>x5?4!n zLgIQ0Gi!UKpKK6;iYt{TcVGJdmza7h1l^OHeH(`ECoPc%`+HzCdc$Kuo zm87$F9Ah=6Huf56X=sa}y=rQWW^5m{3()R1wbr4K^5kjZ`zGnEt6}VRQ~cF%n4cEQ zzDc^8p>=Wb*>E+4J#hM}ma!$KlLt?x^^m(PiYYf0w}h@spR=foyx_d#1~YM+5MP8i%}mxh6z1neAGD*;tm4S=%k#oFl5~EMi@t_MIWLw$8-!-9!9fIn zF7}KhU5+~#yN8RXu2w@BhR}pRwRbgeCl!5;cmmMgC$<9i_KBl_GZ)0g m>q+O*L-uNoy@Ay_BpPlcmAa$ihc}YNr59ntMPmQ#;eP?QY<-*n diff --git a/Debug/Core/Src/MadgwickAHRS.su b/Debug/Core/Src/MadgwickAHRS.su index 8cb7084..a9de0f1 100644 --- a/Debug/Core/Src/MadgwickAHRS.su +++ b/Debug/Core/Src/MadgwickAHRS.su @@ -1,4 +1,4 @@ ../Core/Src/MadgwickAHRS.c:42:6:MadgwickAHRS_init 24 static ../Core/Src/MadgwickAHRS.c:55:6:MadgwickAHRS_update 208 static -../Core/Src/MadgwickAHRS.c:161:6:MadgwickAHRS_update_IMU 144 static -../Core/Src/MadgwickAHRS.c:240:7:invSqrt 32 static +../Core/Src/MadgwickAHRS.c:169:6:MadgwickAHRS_update_IMU 152 static +../Core/Src/MadgwickAHRS.c:257:7:invSqrt 32 static