• ff3324
    了解作者
  • matlab
    开发工具
  • 3KB
    文件大小
  • rar
    文件格式
  • 0
    收藏次数
  • 10 积分
    下载积分
  • 17
    下载次数
  • 2019-05-08 10:40
    上传日期
利用加速度计和磁传感器对姿态算法进行解算,互补滤波算法,
姿态解算算法验证.rar
  • 姿态解算算法验证
  • 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
评论
    相关推荐