├── EKF-AHRS ├── EKF_AHRS.m ├── Get_Init_AHRS.m ├── Get_Yaw.m ├── datasets │ └── ADIS16465-rm3100-2020-01-11-18-57-yaw90.txt └── utils │ ├── CalAddAngle_Zcoord.m │ ├── CalAddRad_Zcoord.m │ ├── CalEndAngle_Zcoord.m │ ├── CalEndRad_Zcoord.m │ ├── quatfromeuler.m │ └── quattoeuler.m ├── README.md └── result ├── pitch.png ├── roll.png └── yaw.png /EKF-AHRS/EKF_AHRS.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/waihekor/EKF_AHRS/ef6192685eb14510650198fead3f23a44fadcc8e/EKF-AHRS/EKF_AHRS.m -------------------------------------------------------------------------------- /EKF-AHRS/Get_Init_AHRS.m: -------------------------------------------------------------------------------- 1 | function [pitch0,roll0,yaw0] = Get_Init_AHRS(acce_data,mag_data) 2 | pitch0 = asin(acce_data(2)/norm(acce_data,2)); 3 | roll0 = atan2(-acce_data(1),acce_data(3)); 4 | 5 | m_x = mag_data(1)*cos(roll0)+mag_data(3)*sin(roll0); 6 | m_y = mag_data(1)*sin(pitch0)*sin(roll0)+ mag_data(2)*cos(pitch0)-mag_data(3)*cos(roll0)*sin(pitch0); 7 | yaw0 = atan2(m_y,m_x); 8 | end 9 | -------------------------------------------------------------------------------- /EKF-AHRS/Get_Yaw.m: -------------------------------------------------------------------------------- 1 | function [yaw0] = Get_Yaw(pitch_angle,roll_angle,mag_data) 2 | 3 | pitch = pitch_angle*pi/180; 4 | roll = roll_angle*pi/180; 5 | m_x = mag_data(1)*cos(roll)+mag_data(3)*sin(roll); 6 | m_y = mag_data(1)*sin(pitch)*sin(roll)+ mag_data(2)*cos(pitch)-mag_data(3)*cos(roll)*sin(pitch); 7 | yaw0 = atan2(m_y,m_x)*180/pi; 8 | end 9 | -------------------------------------------------------------------------------- /EKF-AHRS/utils/CalAddAngle_Zcoord.m: -------------------------------------------------------------------------------- 1 | function add_angle = CalAddAngle_Zcoord( begin_angle,end_angle ) 2 | begin_rad = begin_angle*pi/180; 3 | end_rad = end_angle*pi/180 ; 4 | begin_matrix = [cos(begin_rad),-sin(begin_rad),0;sin(begin_rad),cos(begin_rad),0;0,0,1]; 5 | end_xy = [cos(end_rad),sin(end_rad),0]'; 6 | add_xy = begin_matrix'* end_xy; 7 | add_rad = atan2(add_xy(2), add_xy(1)); 8 | add_angle = add_rad/pi*180; 9 | 10 | end 11 | -------------------------------------------------------------------------------- /EKF-AHRS/utils/CalAddRad_Zcoord.m: -------------------------------------------------------------------------------- 1 | function add_rad = CalAddRad_Zcoord( begin_rad,end_rad ) 2 | begin_matrix = [cos(begin_rad),-sin(begin_rad),0;sin(begin_rad),cos(begin_rad),0;0,0,1]; 3 | end_xy = [cos(end_rad),sin(end_rad),0]'; 4 | add_xy = begin_matrix'* end_xy; 5 | add_rad = atan2(add_xy(2), add_xy(1)); 6 | end 7 | -------------------------------------------------------------------------------- /EKF-AHRS/utils/CalEndAngle_Zcoord.m: -------------------------------------------------------------------------------- 1 | function end_angle = CalEndAngle_Zcoord( begin_angle,add_angle ) 2 | begin_rad = begin_angle*pi/180; 3 | add_rad = add_angle*pi/180 ; 4 | add_matrix = [cos(add_rad),-sin(add_rad),0;sin(add_rad),cos(add_rad),0;0,0,1]; 5 | begin_xy = [cos(begin_rad),sin(begin_rad),0]'; 6 | end_xy = add_matrix*begin_xy; 7 | end_rad = atan2(end_xy(2),end_xy(1)); 8 | end_angle = end_rad/pi*180; 9 | end 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /EKF-AHRS/utils/CalEndRad_Zcoord.m: -------------------------------------------------------------------------------- 1 | function end_rad = CalEndRad_Zcoord( begin_rad,add_rad ) 2 | 3 | add_matrix = [cos(add_rad),-sin(add_rad),0;sin(add_rad),cos(add_rad),0;0,0,1]; 4 | begin_xy = [cos(begin_rad),sin(begin_rad),0]'; 5 | end_xy = add_matrix*begin_xy; 6 | end_rad = atan2(end_xy(2),end_xy(1)); 7 | 8 | end 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /EKF-AHRS/utils/quatfromeuler.m: -------------------------------------------------------------------------------- 1 | 2 | function [w,x,y,z]=quatfromeuler( pitch,roll,yaw) 3 | w = cos(pitch/2)*cos(roll/2)*cos(yaw/2) - sin(pitch/2)*sin(roll/2)*sin(yaw/2); 4 | x = cos(roll/2)*cos(yaw/2)*sin(pitch/2) - cos(pitch/2)*sin(roll/2)*sin(yaw/2); 5 | y = cos(pitch/2)*cos(yaw/2)*sin(roll/2) + cos(roll/2)*sin(pitch/2)*sin(yaw/2); 6 | z = cos(pitch/2)*cos(roll/2)*sin(yaw/2) + cos(yaw/2)*sin(pitch/2)*sin(roll/2); -------------------------------------------------------------------------------- /EKF-AHRS/utils/quattoeuler.m: -------------------------------------------------------------------------------- 1 | function [pitch,roll,yaw] = quattoeuler(q) 2 | rad2deg=180/pi; 3 | R_n2b=[ 1 - 2 * (q(4) *q(4) + q(3) * q(3)) 2 * (q(2) * q(3)-q(1) * q(4)) 2 * (q(2) * q(4) +q(1) * q(3)) ; 4 | 2 * (q(2) * q(3) +q(1) * q(4)) 1 - 2 * (q(4) *q(4) + q(2) * q(2)) 2 * (q(3) * q(4)-q(1) * q(2)); 5 | 2 * (q(2) * q(4)-q(1) * q(3)) 2 * (q(3) * q(4)+q(1) * q(2)) 1 - 2 * (q(2) *q(2) + q(3) * q(3))];%cnb 6 | 7 | pitch = asin(R_n2b(3,2))*rad2deg; 8 | roll = atan2(-R_n2b(3,1),R_n2b(3,3))*rad2deg; 9 | yaw = atan2(-R_n2b(1,2),R_n2b(2,2))*rad2deg; 10 | end 11 | 12 | 13 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # EKF_AHRS 2 | ![image](https://github.com/waihekor/EKF_AHRS/blob/master/result/pitch.png) 3 | ![image](https://github.com/waihekor/EKF_AHRS/blob/master/result/roll.png) 4 | ![image](https://github.com/waihekor/EKF_AHRS/blob/master/result/yaw.png) 5 | 6 | 7 | -------------------------------------------------------------------------------- /result/pitch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/waihekor/EKF_AHRS/ef6192685eb14510650198fead3f23a44fadcc8e/result/pitch.png -------------------------------------------------------------------------------- /result/roll.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/waihekor/EKF_AHRS/ef6192685eb14510650198fead3f23a44fadcc8e/result/roll.png -------------------------------------------------------------------------------- /result/yaw.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/waihekor/EKF_AHRS/ef6192685eb14510650198fead3f23a44fadcc8e/result/yaw.png --------------------------------------------------------------------------------