[Paparazzi-devel] New Filter equations in ahrs_int_cmpl_euler.c

[flavio_just]

Hello guys!
I have a complex question about developing a new filter, maybe it's even
more a question about C itself than PPRZ. I developed a new filter based on
the theory of a matrix trace. It's called Trace Based Filter (TBF). I was
hoping I could implement it on PPRZ. For now I am trying to do it inside
ahrs_int_cmpl_euler.c, inside propagate function (since it is the current
filter installed on my drone). The problem is that I have some recursive
equations that I don't know how I can implement them, I tried several ways
and the results are incorrect. I will leave the equations that I currently
have on Matlab:

 RotTerm=(a*delta_t/2)*(D*Ry'*R_hat-R_hat'*Ry*D);
  
RotCorr=[ -RotTerm(2,3), RotTerm(1,3), -RotTerm(1,2) ]';
ew=ew-delta_t*DD*ew-RotCorr;
Corr=ew+R_hat'*Ry*gyro(k,:)';
    
Corr_s=[0       -Corr(3)   Corr(2);
        Corr(3)    0       -Corr(1);
        -Corr(2)   Corr(1)    0];
    
    
R_hat=R_hat*expm(Corr_s.*delta_t);
   

 %here apply EULERS OF RMAT to R_hat
    phi=atan2(R_hat(2,3),R_hat(3,3));
    theta=-asin(R_hat(1,3));
    psi=atan2(R_hat(1,2),R_hat(1,1));

Basically R_hat and Ry are matrixes (Ry is a rotation matrix obtain directly
from the sensors through euler angles) and R_hat is the estimated Rotation
matrix to obtain estimated eulers,D and DD are diagonal matrixes, 'a' and
delta_t are scalars. As you guys can see, the first equation for RotTerm and
the one for Corr are depend of previous values of R_hat and ew. I created
all of the necessary values in ahrs.h. For R_hat and ew I assigned them zero
value in init_function but it doesn't seem to work. I have problem too in
computing matrix exponentional in C, is there any function?

I know it's a complex question but it could be useful in the future for me
and for PPRZ eventually. The TBF is already validated in my Master Thesis. 

In annex I'm leaving you guys the file ahrs_int_cmpl_euler.c and
float_pprz_algebra.h where I wrote some functions to reproduce the
equations, if it helps. 

Best regards and thanks!
Flávio Justino

ahrs_int_cmpl_euler.c
<http://lists.paparazziuav.org/file/n17555/ahrs_int_cmpl_euler.c>  
ahrs_int_cmpl_euler.h
<http://lists.paparazziuav.org/file/n17555/ahrs_int_cmpl_euler.h>  
ahrs.h <http://lists.paparazziuav.org/file/n17555/ahrs.h>  
pprz_algebra_float.h
<http://lists.paparazziuav.org/file/n17555/pprz_algebra_float.h>  





--
View this message in context: 
http://lists.paparazziuav.org/New-Filter-equations-in-ahrs-int-cmpl-euler-c-tp17555.html
Sent from the paparazzi-devel mailing list archive at Nabble.com.