梯度下降方法的磁陀螺全姿态数据融合方法仿真
算法采用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 = MARbete3( ar,mag,deltat,beta,zeta )
%磁陀螺组合测量仿真研究
% ar=三轴角速率
% mag=三轴磁数据
% deltat: 采样率
SEq = ; % quaternion initial conditions
wb = ;
Angle(1,:) = ;
% 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=
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)=;
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 点击此处下载 ourdev_718729VU783S.zip(文件大小:2K) (原文件名:MAR.zip)
上面有main.m 产生数据及运行算法
MARbete3.m 算法函数
直接运行main.m 即可看到仿真结果，但是仿真失败，还再找原因，请教各位 对这个比较感兴趣，能不能说说你的算法的核心思想 技术大牛！mark下再看！！ 回复【2楼】AirPig空中飞猪
对这个比较感兴趣，能不能说说你的算法的核心思想
-----------------------------------------------------------------------
這篇我研究了好一陣子，但是文章寫得很隱晦，反而看P.11那個圖比較好懂。比較難理解的是上面加速規輸入的J和f的含意。可以從Source Code去猜他的整個算法過程。可以看出來是將f裡面的q(四元素)以加速規估測角度角度函數f，▽f/|▽f|就是f函數曲面的梯度值，就不準確的直觀看來可以是以加速規到加速規估測角速度的東西，在乘以β反饋到陀螺移輸入修正陀螺的offset。
但是程式碼對照數學的部分有些很跳躍，可能需要完整的thesis才能補齊中間消失的資訊。後面加入電羅盤的做法也差不多，只是多一道反饋的程序。精神還是在ζ和β的選定，這個跟類神經的學習系數一樣，選的好不好差很多。可以參考P.14的作者選定的方法或是自己去try一個好的係數。作者的公式不是最好係數，但事是滿合理的係數計算方式。
我沒試過仿真，不過這個程式碼我移植到硬體上是可以用的，他的動靜態RMS都落在文獻的範圍。基本上沒問題。樓主仿真可能參數有下錯!! 这个又看了一个星期，似乎有点明白其中的原因。我的仿真之所以不成功，最关键的是，我只用到了地磁，而没有使用加速度计。我们再仔细回头看下，梯度下降法，对什么最有效？是对圆，而我生成的数据，是静止的，是线，所以，仿真失败。用地磁和加速度计组合，再使用梯度下降，这个组合非常的棒。加速度计在准静态的时候才是非常有效的，而在动态的时候，地磁是非常有效的。所以这个组合比较成功，简单，高效，对于四轴飞行够了！ 回复【4楼】g921002
回复【2楼】airpig空中飞猪
对这个比较感兴趣，能不能说说你的算法的核心思想
-----------------------------------------------------------------------
▽f/|▽f|就是f函數曲面的梯度值，
-----------------------------------------------------------------------
这个我从数学上很难理解，但是他这样做却是比较有效 回复【6楼】caopeng32
▽f/|▽f|就是f函數曲面的梯度值，
-----------------------------------------------------------------------
这个我从数学上很难理解，但是他这样做却是比较有效
-----------------------------------------------------------------------
這是我的看法，f函數的梯度▽f可以想像成f變化的"趨勢"，雖然這篇文章的CODE的演算過程式離散的，其實不過是把
f變化的趨勢可以看做四元素空間變化的向量，再透過β反饋到陀螺移估測四元素，直觀來說可以等同於Complementary Filter的變形應用。 Sebastian O.H. Madgwick的梯度下降磁陀螺数据融合方法ourdev_723287PURE37.zip(文件大小:3K) (原文件名:MAR.zip)
程序说明同一楼。但是本次数据融合的结果令人震惊，是否有错误，大家自己可以验证下~~~~~ 1楼与8楼的程序区别说明：
1）由静止数据改为单轴旋转
2）由Eb=改为Eb= 正在学，懂了就写出来。 9传感器数据融合的核心，应该都是权值的选择 感谢楼主分享，希望楼主能给我们这些初学者上传点资料，特别是卡尔曼滤波的。谢谢
页:
[1]