AHRSupdate.rar

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 采用AHRS进行九轴陀螺仪数据的融合处理 % 2017-12-25 %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [q0out,q1out,q2out,q3out,yaw,pitch,roll] = AHRSupdate(gx,gy,gz,ax,ay,az,mx,my,mz,dT) global q0 q1 q2 q3 global exInt eyInt ezInt % 调整系数 temp = gx^2+gy^2+gz^2; if temp < 0.314 Kp = 1; Ki = dT/2; elseif temp >= 0.314 && temp < 0.6 Kp = 0.05; Ki = 0; else Kp = 1; Ki = 0.05; end halfT = dT/2; % 中间变量 q0q0 = q0^2; q0q1 = q0*q1; q0q2 = q0*q2; q0q3 = q0*q3; q1q1 = q1^2; q1q2 = q1*q2; q1q3 = q1*q3; q2q2 = q2^2; q2q3 = q2*q3; q3q3 = q3^2; % 归一化 temp = norm([ax ay az]); if temp ~= 0 ax = ax/temp; ay = ay/temp; az = az/temp; end temp = norm([mx my mz]); if temp ~= 0 mx = mx/temp; my = my/temp; mz = mz/temp; end % 计算参考磁通方向 hx = 2*mx*(0.5-q2q2-q3q3)+2*my*(q1q2-q0q3)+2*mz*(q1q3+q0q2); hy = 2*mx*(q1q2+q0q3)+2*my*(0.5-q1q1-q3q3)+2*mz*(q2q3+q0q1); hz = 2*mx*(q1q3-q0q2)+2*my*(q2q3+q0q1)+2*mz*(0.5-q1q1-q2q2); bx = norm([hx hy]); bz = hz; % 四元数反推磁场强度和重力加速度 vx = 2*(q1q3-q0q2); vy = 2*(q0q1+q2q3); vz = q0q0-q1q1-q2q2+q3q3; wx = 2*bx*(0.5-q2q2-q3q3)+2*bz*(q1q3-q0q2); wy = 2*bx*(q1q2-q0q3)+2*bz*(q0q1+q2q3); wz = 2*bx*(q0q2+q1q3)+2*bz*(0.5-q1q1-q2q2); % 测量和反推误差 ex = -((ay*vz-az*vy) + (my*wz-mz*wy)); ey = -((az*vx-ax*vz) + (mz*wx-mx*wz)); ez = -((ax*vy-ay*vx) + (mx*wy-my*wx)); % ex = ((ay*vz-az*vy) + (my*wz-mz*wy)); % ey = ((az*vx-ax*vz) + (mz*wx-mx*wz)); % ez = ((ax*vy-ay*vx) + (mx*wy-my*wx)); % 滤波 exInt = exInt + ex*Ki; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki; % 陀螺仪修正 gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; % 四元数更新 temp0 = q0 + (-q1*gx-q2*gy-q3*gz)*halfT; temp1 = q1 + (q0*gx+q2*gz-q3*gy)*halfT; temp2 = q2 + (q0*gy-q1*gz+q3*gx)*halfT; temp3 = q3 + (q0*gz+q1*gy-q2*gz)*halfT; q0 = temp0; q1 = temp1; q2 = temp2; q3 = temp3; temp = norm([q0 q1 q2 q3]); q0 = q0/temp; q1 = q1/temp; q2 = q2/temp; q3 = q3/temp; % 姿态角 yaw = atan2(2*q0*q3+2*q1*q2,1-2*q2*q2-2*q3*q3)/3.14*180; pitch = asin(2*q0*q2-2*q1*q3)/3.14*180; roll = atan2(2*q0*q1+2*q2*q3,1-2*q1*q1-2*q2*q2)/3.14*180; q0out = q0; q1out = q1; q2out = q2; q3out = q3;