微信:HuangL1121
QQ号:1224848052
团队:MATLAB代做|MATLAB专业代做|FPGA代做|FPGA专业代做
地址:北京市-朝阳区-双柳北街18号院
当前位置:首页 > 技术资讯 > MATLAB代做|四元数法捷联惯导解算算法

MATLAB代做|四元数法捷联惯导解算算法

clear 
% close 
glvs 
ts = 0.1;        %采样周期 
n = 3;    % 子样数 
att0=[0;0;0]*glv.deg; vn0=[0;0;0]; pos0=[0*glv.deg;0;0];   %初始值设置 
qnb0 = a2qnb(att0); 
vn = vn0; pos = pos0; 
[wnie, wnen, RMh, RNh, gn] = earth(pos, vn0); 
wbib = qmulv(qconj(qnb0),wnie); 
fb = qmulv(qconj(qnb0),-gn); 
wm=wbib*ts; vm=fb*ts;   %静止时角增量和比力增量 
eb = [0.01; 0.01; 0.01]*glv.dph; web =  [0.001; 0.001; 0.001]*glv.dph; % 陀螺误差 
db =  [100; 100; 100]*glv.ug;  wdb =  [10; 10; 10]*glv.ug; % 加速度计误差 
phi = [0.5;0.5;3]*glv.min; qnb = qaddphi(qnb0,phi); % 加入姿态误差 
kk = 1; 
t = 3600;  % 总时间长度 
len = fix(t/ts/n); errphi=zeros(len,3); errvn=errphi; errpos=errphi; 
for k=0:n:len*n 
    for k1=1:n 
        wme(:,k1) = wm+(eb+web.*randn(3,1))*ts;  % 构造多子样数据 
        vme(:,k1) = vm+(db+wdb.*randn(3,1))*ts; 
    end 
    [qnb,vn,pos]=sins(qnb,vn,pos,wme,vme,ts);  
    vn(3) = 0;  % 限定高度发散 
    errphi(kk,:) = qq2phi(qnb,qnb0)'; %求姿态误差 
    errvn(kk,:) = (vn-vn0)'; 
    errpos(kk,:) = (pos-pos0)'; 
    kk = kk+1; 
end 
time = [1:length(errphi)]*ts*n; 
figure, 
subplot(3,1,1),plot(time,errphi/glv.min), ylabel('\it\phi\rm / arcmin'); grid on 
subplot(3,1,2),plot(time,errvn), ylabel('\it\delta V\rm / m/s');  grid on 
subplot(3,1,3),plot(time,[errpos(:,1:2)*glv.Re,errpos(:,3)]), ylabel('\it\delta P\rm / m'); grid on 
xlabel('\itt\rm / s'); 

联系:highspeedlogic

QQ :1224848052

微信:HuangL1121

邮箱:1224848052@qq.com

网站:http://www.mat7lab.com/

网站:http://www.hslogic.com/

微信扫一扫:



上一篇:MATLAB专业代做|MATLAB淘宝代做|MATLAB SCI代做小波变换阈值去噪的MATLAB源代码

版权所有: MATLAB代做|MATLAB专业代做|FPGA代做|FPGA专业代做 Copyright © 2018 All rights reserved.
微信:HuangL1121 QQ:1224848052
地址:北京市-朝阳区-双柳北街18号院

扫一扫,关注我们