@Revanth Kumar Adireddy Hi, I have given here the synthetic simulation data generated by imuSensor and then recovered the attitude data through the filter and simulated it.
%% 定义滤波器
SampleRate = 100;
fuse1 = imufilter(ReferenceFrame='NED',SampleRate=SampleRate,GyroscopeNoise=1e-4, GyroscopeDriftNoise=1e-6);
% fuse2 = ahrsfilter('SampleRate', SampleRate); % thiS filter must need magnetometer
fuse3 = complementaryFilter(ReferenceFrame='NED',HasMagnetometer=false,AccelerometerGain=0.01);
%% 定义人工生成的groundTruth
% 分别在2秒内绕x,y,z轴旋转360度，得到真值仿真数据
fs = SampleRate;
eachLoopNumSamples = fs*2;
totalNumSamples = 3*eachLoopNumSamples;
traj = kinematicTrajectory('SampleRate',fs);
accBody = zeros(totalNumSamples,3);
angVelBody = zeros(totalNumSamples,3);
angVelBody(1:eachLoopNumSamples,1) = (2*pi)/2;
angVelBody(eachLoopNumSamples+1:2*eachLoopNumSamples,2) = (2*pi)/2;
angVelBody(2*eachLoopNumSamples:end,3) = (2*pi)/2;
[~,orientationNED,~,accNED,angVelNED] = traj(accBody,angVelBody);
IMU = imuSensor('accel-gyro','SampleRate',fs);
% % 加点噪音模拟真实的IMU
% IMU.Accelerometer = accelparams(...
% MeasurementRange=20,...
% Resolution=0.00239,...
% TemperatureScaleFactor=0.008,...
% ConstantBias=0.1962,...
% TemperatureBias=0.0015,...
% NoiseDensity=0.1);
[accelReadings,gyroReadings] = IMU(accNED,angVelNED,orientationNED);
t = (0:(totalNumSamples-1))/SampleRate;
plot(t,accelReadings)
legend('X-axis','Y-axis','Z-axis')
ylabel('Acceleration (m/s^2)')
title('Accelerometer Readings')
%% 展示姿态解算恢复结果
fig = figure;
ax = axes(Parent=fig);
pp = poseplot(ax,"NED");
xlabel("North-x (m)")
ylabel("East-y (m)")
zlabel("Down-z (m)");
for idx = 1:size(accelReadings,1)
% Get the latest acceleration and angular velocity data
acc = accelReadings(idx,:);
gyr = gyroReadings(idx,:);
if ~isempty(acc) && ~isempty(gyr)
% acc = -[acc(2),acc(1),-acc(3)]; % if data from matlab mobile™ APP
% gyr = [gyr(2),gyr(1),gyr(3)];
% Use the imufilter to estimate orientation (quaternion)
orientation = fuse3(acc, gyr);
% Convert quaternion to rotation matrix
rotm = quat2rotm(orientation);
set(pp, "Orientation", orientation);
end
% Refresh the plot
% drawnow ;
pause(1/SampleRate);
end
