Supporting Material for EKF_AHRS.m

[wkyoun]

Dear NicoChou.

Thank you for the great matlab code for EKF_AHRS. I have been studying EKF_ARHS, and found your code.

https://github.com/NicoChou/ESKF-Attitude-Estimation/blob/master/EKF_AHRS.m

I seems to me that your matlab code is working correctly, However, I have difficulty in interpreting your matlab code. (Even though I am familiar with kalman filter)

I have following questions regarding following equations, Is there any supporting material which describe the mathmetical equation for following sections as you did for error-state EKF?

https://github.com/NicoChou/ESKF-Attitude-Estimation/blob/master/EKF_AHRS.m#L38 https://github.com/NicoChou/ESKF-Attitude-Estimation/blob/master/EKF_AHRS.m#L41 https://github.com/NicoChou/ESKF-Attitude-Estimation/blob/master/EKF_AHRS.m#L44

https://github.com/NicoChou/ESKF-Attitude-Estimation/blob/master/EKF_AHRS.m#L78-L81

https://github.com/NicoChou/ESKF-Attitude-Estimation/blob/master/EKF_AHRS.m#L87-L94

https://github.com/NicoChou/ESKF-Attitude-Estimation/blob/master/EKF_AHRS.m#L117-L119

[yuzhou42]

Hi won3y,

For error-state EKF you can check this.

For EKF_AHRS, sorry I don't have supporting material now. I choose attitude quaternion and gyro bias as the state vector. Accelerate and compass data as observation vector. So you can deduced L78-L81 as the state equation. L87-L94 can be deduced from L78-L84. Other equations can be found in any inertial navigation book.

[wkyoun]

Dear ZhouYu. Thank you for the kind reply.

However, I have question regarding following part, I don't understand why you used h[2], h[3] for L100,

and how the matlab code for L101 is derived.

And for L119, what is the meaning of -8.3?

https://github.com/NicoChou/ESKF-Attitude-Estimation/blob/master/EKF_AHRS.m#L100 b = [0 norm([h(2) h(3)]) 0 h(4)];

https://github.com/NicoChou/ESKF-Attitude-Estimation/blob/master/EKF_AHRS.m#L101

  hk_2 = [(2*b(2)*(0.5 - x_(3)^2 - x_(4)^2) + 2*b(4)*(x_(2)*x_(4) - x_(1)*x_(3))),2*b(2)*(x_(2)*x_(3) - x_(1)*x_(4)) + 2*b(4)*(x_(1)*x_(2) + x_(3)*x_(4)) ,2*b(2)*(x_(1)*x_(3) +x_(2)*x_(4)) + 2*b(4)*(0.5 - x_(2)^2 - x_(3)^2)];%T1_WtoB磁场[m,0,0]

https://github.com/NicoChou/ESKF-Attitude-Estimation/blob/master/EKF_AHRS.m#L119 yawEKF(k) = atan2(2 (xx(k) y(k) + zz(k) w(k)) , 1 - 2 (zz(k) zz(k) + y(k) y(k)))*rad_deg-8.3;

[yuzhou42]

Hi won13y, I'm glad I can help. For L100-L109, you can check this madgwick_internal_report . The report can answer all your question about those equations. The number 8.3 is the magnetic declination in my city. You can get the data in your city from this web.

[wkyoun]

Thank you very much for your help. I would really appreciate your help.

Howver, I have question for L78 ~ L81, It seems to me that you used the equation (13) from madgwick_internal_report for L78 ~ L81, https://github.com/NicoChou/ESKF-Attitude-Estimation/blob/master/EKF_AHRS.m#L78-L81

However, I am trying to change the code for L78 ~ L81 into equation (183) in page 40 from following reference.

http://www.iri.upc.edu/people/jsola/JoanSola/objectes/notes/kinematics.pdf

Acutally, I have tried to change not only the state update equation but also jacobian, but EKF fail.

It seems to me that both equation is only different in terms of intergration implementation, thus EKF should not fail.

[yuzhou42]

These two equations are different... You can read those material again.

[wkyoun]

@NicoChou

How have you been? I was able to modify your EKF and ESKF, and I think that I understand completely.

One thing that I am not confident is as following part

https://github.com/NicoChou/ESKF-Attitude-Estimation/blob/master/EKF_AHRS.m#L100

b = [0 norm([h(2) h(3)]) 0 h(4)];

Is the East of magnetic field in earth frame in your region(where you have gather the dataset) ``zero```?

https://github.com/NicoChou/ESKF-Attitude-Estimation/blob/master/EKF_AHRS.m#L99

In other word, why the earth magnetic field h = quaternProd(x_(1:4), quaternProd([0 mag(k,:)], quatInv(x_(1:4)'))); is changed to b = [0 norm([h(2) h(3)]) 0 h(4)];?

It looks like that the effect of an erroneous inclination of the measured direction earth's magnetic field, was corrected if the filter's reference direction of the earth's magnetic fields of the same inclination as mentioned in madgwick_internal_report chapter 3.4. Am I right?

Would you please let me know the latitude and longitude of location where dataset was obtained so that I will be able to know magnetic field in earth frame?

[yuzhou42]

Hi Wonkeun, I have been good. Thanks for asking. Yes, you are right. That is a compensation for magnetic distortion. The dataset was collected in Shen Yang, China.

[wkyoun]

@NicoChou

Thank you for the reply.

I cannot understand completely why the following equation h = quaternProd(x_(1:4), quaternProd([0 mag(k,:)], quatInv(x_(1:4)'))); is changed to b = [0 norm([h(2) h(3)]) 0 h(4)]; is used for compensation for magnetic distortion for inclination.

Do you have any other supporting material for compensation for magnetic distortion other than madgwick_internal_report for describing above manipulation?