|
算法采用http://code.google.com/p/imumargalgorithm30042010sohm/的数据融合方法,但是仿真失败,不知原因,请教各位看看:
%产生静止试验数据
clear,clc;
deltat = 0.002; % 采样周期
n = 50000; %采样点数
Error = 0.33;%测量误差
Merror = normrnd(0,Error,n,3);
Drift = 0.014; %漂移
for i = 1:n
ar(i,:)=Merror(i,:)+(i-1)*Drift*deltat;%角速率测量值
end
mag = ones(n,3);%产生一个全1的固定磁测量值
beta = sqrt(3/4)*pi*Error/180/15;
zeta = sqrt(3/4)*pi*Drift/180/15;
%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Angle,b] = MARbete3( ar,mag,deltat,beta,zeta )
%磁陀螺组合测量仿真研究
% ar=[wx wy wz]三轴角速率
% mag=[mx my mz]三轴磁数据
% deltat: 采样率
SEq = [1 0 0 0]; % quaternion initial conditions
wb = [0,0,0];
Angle(1,:) = [0 0 0];
% estimated direction of earth's magnetic field in the earth frame
bx = 1;bz = 0;
%
h = deltat;
n = size(mag,1);
%%
for i=1:n
%m = MagnetometerCalibration(mag(i,:)); % m=[mx my mz]
m = mag(i,:);
mx = m(1);
my = m(2);
mz = m(3);
m = m/norm(m);
% Compute the gradient
f=[2*bx*(0.5-SEq(3)*SEq(3)-SEq(4)*SEq(4)) + 2*bz*(SEq(2)*SEq(4)-SEq(1)*SEq(3)) - m(1);
2*bx*(SEq(2)*SEq(3)-SEq(1)*SEq(4)) + 2*bz*(SEq(1)*SEq(2)+SEq(3)*SEq(4)) - m(2);
2*bx*(SEq(1)*SEq(3)+SEq(2)*SEq(4)) + 2*bz*(0.5-SEq(2)*SEq(2)-SEq(3)*SEq(3))-m(3)];
J=[-2*bz*SEq(3), 2*bz*SEq(4), -4*bx*SEq(3)-2*bz*SEq(1), -4*bx*SEq(4)+2*bz*SEq(2);
-2*bx*SEq(4)+2*bz*SEq(2), 2*bx*SEq(3)+2*bz*SEq(1), 2*bx*SEq(2)+2*bz*SEq(4), -2*bx*SEq(1)+2*bz*SEq(3);
2*bx*SEq(3), 2*bx*SEq(4)-4*bz*SEq(2), 2*bx*SEq(1)-4*bz*SEq(3), 2*bx*SEq(2)];
SEqHatDot = (J'*f)';
% Normalise the gradient
if norm(SEqHatDot) ~= 0
SEqHatDot = SEqHatDot/norm(SEqHatDot);
end
% compute angular estimated direction of the gyroscope error
werr(1) = 2*SEq(1)*SEqHatDot(2)-2*SEq(2)*SEqHatDot(1)-2*SEq(3)*SEqHatDot(4)+2*SEq(4)*SEqHatDot(3);
werr(2) = 2*SEq(1)*SEqHatDot(3)+2*SEq(2)*SEqHatDot(4)-2*SEq(3)*SEqHatDot(1)-2*SEq(4)*SEqHatDot(2);
werr(3) = 2*SEq(1)*SEqHatDot(4)-2*SEq(2)*SEqHatDot(3)+2*SEq(3)*SEqHatDot(2)-2*SEq(4)*SEqHatDot(1);
%compute and remove the gyroscope baises
wb = wb+werr*h*zeta;
b(i,:)=wb;
%
w = ar(i,:)*pi/180/15;
w = w-wb;
% Compute the quaternion derivative measured by gyroscopes
SEqDot = [-0.5*SEq(2)*w(1) - 0.5*SEq(3)*w(2) - 0.5*SEq(4)*w(3);
0.5*SEq(1)*w(1) + 0.5*SEq(3)*w(3) - 0.5*SEq(4)*w(2);
0.5*SEq(1)*w(2) - 0.5*SEq(2)*w(3) + 0.5*SEq(4)*w(1);
0.5*SEq(1)*w(3) + 0.5*SEq(2)*w(2) - 0.5*SEq(3)*w(1)]';
% Compute then integrate the estimated quaternion derrivative
SEq = SEq+(SEqDot-beta*SEqHatDot)*h;
% Normalise quaternion
SEq = SEq/norm(SEq);
% compute flux in the earth frame
h_x = 2*mx*(0.5-SEq(3)*SEq(3)-SEq(4)*SEq(4)) + 2*my*(SEq(2)*SEq(3)-SEq(1)*SEq(4)) + 2*mz*(SEq(2)*SEq(4)+SEq(1)*SEq(3));
h_y = 2*mx*(SEq(2)*SEq(3)+SEq(1)*SEq(4)) + 2*my*(0.5-SEq(2)*SEq(2)-SEq(4)*SEq(4)) + 2*mz*(SEq(3)*SEq(4)-SEq(1)*SEq(2));
h_z = 2*mx*(SEq(2)*SEq(4)-SEq(1)*SEq(3)) + 2*my*(SEq(3)*SEq(4)+SEq(1)*SEq(2)) + 2*mz*(0.5-SEq(2)*SEq(2)-SEq(3)*SEq(3));
% normalise the flux vector to have only components in the x and z
bx = sqrt((h_x * h_x) + (h_y * h_y));
bz = h_z;
% Compute the Euler angles from the quaternion
pitch = atan2(2*SEq(2)*SEq(3)-2*SEq(1)*SEq(4),2*SEq(1)*SEq(1)+2*SEq(2)*SEq(2)-1);
yaw = -asin(2*SEq(2)*SEq(4)+2*SEq(1)*SEq(3));
roll = atan2(2*SEq(3)*SEq(4)-2*SEq(1)*SEq(2), 2*SEq(1)*SEq(1)+2*SEq(4)*SEq(4)-1);
% Calculate the new Euler angles, Convert from radians to degrees.
Angle(i,1:3)=[pitch, yaw, roll];
end
%%
Angle = Angle*57.2958;
plot(h:h:n*h,Angle(:,1),'b',h:h:n*h,Angle(:,2),'r',h:h:n*h,Angle(:,3),'g');%%
xlabel('时间/s');
ylabel('角度/°');
end |
阿莫论坛20周年了!感谢大家的支持与爱护!!
知道什么是神吗?其实神本来也是人,只不过神做了人做不到的事情 所以才成了神。 (头文字D, 杜汶泽)
|