# 姿态解算算法验证.rar

• ff3324
了解作者
• matlab
开发工具
• 3KB
文件大小
• rar
文件格式
• 0
收藏次数
• 10 积分
下载积分
• 17
下载次数
• 2019-05-08 10:40
上传日期

• 姿态解算算法验证
• New Folder
• filter_z.m
3.1KB
• produce_data.m
3.7KB
• R2acc_mag.m
108B
• initial.m
1.5KB

% 分别围绕x,y,z轴以1rad/s的速度旋转360度，生成加速度计和陀螺仪的采集数据 clear all %% 定义陀螺仪的采集频率 rate_gyro = 10000; %% 生成数据 num = 1; %num 用于记录生成的数据的数量 roll(num) = 0; pitch(num) = 0; yaw(num) = 0; %初始位置姿态为[0,0,0]； x_rate(num) = 0; y_rate(num) = 0; z_rate(num) = 0; %初始加速度为[0,0,0]; T(num) = 0; %初始时间为0 acc_x(num) = 0; acc_y(num) = 0; acc_z(num) = 9.8; mag_x(num) = 10; mag_y(num) = 0; mag_z(num) = 1; for axis = 1:1:3 %axis为1，2，3时分别表示绕x，y，z轴旋转 for angle = 0:1/rate_gyro:2*pi num = num + 1; switch axis case 1 roll(num) = angle; x_rate(num) = 1; if num > 1 pitch(num) = pitch(num-1); yaw(num) = yaw(num-1); y_rate(num) = 0; z_rate(num) = 0; end case 2 pitch(num) = angle; y_rate(num) = 1; if num > 1 roll(num) = roll(num-1); yaw(num) = yaw(num-1); x_rate(num) = 0; z_rate(num) = 0; end case 3 yaw(num) = angle; z_rate(num) = 1; if num > 1 roll(num) = roll(num-1); pitch(num) = pitch(num-1); x_rate(num) = 0; y_rate(num) = 0; end end if yaw(num) > pi yaw(num) = yaw(num) - 2*pi; end if pitch(num) > pi pitch(num) = pitch(num) - 2*pi; end if roll(num) > pi roll(num) = roll(num) - 2*pi; end R = [cos(yaw(num))*cos(pitch(num)), cos(yaw(num))*sin(pitch(num))*sin(roll(num))-sin(yaw(num))*cos(roll(num)), cos(yaw(num))*sin(pitch(num))*cos(roll(num))+sin(yaw(num))*sin(roll(num)); sin(yaw(num))*cos(pitch(num)), sin(yaw(num))*sin(pitch(num))*sin(roll(num))+cos(yaw(num))*cos(roll(num)), sin(yaw(num))*sin(pitch(num))*cos(roll(num))-cos(yaw(num))*sin(roll(num)); -sin(pitch(num)), cos(pitch(num))*sin(roll(num)), cos(pitch(num))*cos(roll(num));]; [acc, mag] = R2acc_mag(R'); acc_x(num) = acc(1); acc_y(num) = acc(2); acc_z(num) = acc(3); mag_x(num) = mag(1); mag_y(num) = mag(2); mag_z(num) = mag(3); T(num) = T(num-1) + 1/rate_gyro; end end %% 仿真还原（互补滤波） data_num = 1; initial_flag = 0; while 1 Acc.x = acc_x(data_num); Acc.y = acc_y(data_num); Acc.z = acc_z(data_num); Mag.x = mag_x(data_num); Mag.y = mag_y(data_num); Mag.z = mag_z(data_num); Gyro.x = x_rate(data_num); Gyro.y = y_rate(data_num); Gyro.z = z_rate(data_num); if initial_flag == 0 [q0, q1, q2, q3, initial_flag] = initial(Acc,Mag) ; if initial_flag i = 1; Yaw(i) = atan2(2 * q1*q2 + 2 * q0*q3, 1 - 2 * (q2*q2 + q3*q3)) *57.3; Pitch(i) = -asin(2 * (q1*q3 - q0*q2))* 57.3 ; Roll(i) = atan2(2 * q2*q3 + 2 * q0*q1, q0*q0 - q1*q1 - q2*q2 + q3*q3)* 57.3 ; i = i+1; end end if i > 1 [q0, q1, q2, q3, a] = filter_z (q0, q1, q2, q3, Gyro, Acc, Mag, rate_gyro); Yaw(i) = atan2(2 * q1*q2 + 2 * q0*q3, 1 - 2 * (q2*q2 + q3*q3)) *57.3; Pitch(i) = -asin(2 * (q1*q3 - q0*q2))* 57.3 ; Roll(i) = atan2(2 * q2*q3 + 2 * q0*q1, q0*q0 - q1*q1 - q2*q2 + q3*q3)* 57.3 ; i = i+1; end data_num = data_num + 1; if data_num > num break end end

相关推荐