hpro1014.m

自己编写的kalman滤波算法的应用

value=get(handles.pitch,'Value');
if(value==1)
    %set(handles.pitch,'Enable', 'on');
    %set(handles.roll,'Enable', 'off');
    %set(handles.yaw,'Enable', 'off');
%     
     set(handles.pitchvalue,'Enable','on');
     %set(handles.rollvalue,'Enable','off');
     %set(handles.yawvalue,'Enable','off');
%     
     set(handles.pitchfcy,'Enable','on');
     %set(handles.rollfcy,'Enable','off');
     %set(handles.yawfcy,'Enable','off');
%     
     set(handles.pitchorigphase,'Enable','on');
     %set(handles.rollorigphase,'Enable','off');
     %set(handles.yaworigphase,'Enable','off');
     
 else
    % set(handles.pitch,'Enable', 'off');
    %set(handles.roll,'Enable', 'on');
    %set(handles.yaw,'Enable', 'on');
%     
     set(handles.pitchvalue,'Enable','off');
     %$set(handles.rollvalue,'Enable','on');
     %set(handles.yawvalue,'Enable','on');
%     
     set(handles.pitchfcy,'Enable','off');
     %set(handles.rollfcy,'Enable','on');
     %set(handles.yawfcy,'Enable','on');
%     
     set(handles.pitchorigphase,'Enable','off');
     %set(handles.rollorigphase,'Enable','on');
     %set(handles.yaworigphase,'Enable','on');
     
     %%%%2003.12.4
     set(handles.pitchvalue,'String','0');
     set(handles.pitchfcy,'String','0');
     set(handles.pitchorigphase,'String','0');
     %%%%%%%%%%%%
 end


% --------------------------------------------------------------------
function varargout = pitchvalue_Callback(h, eventdata, handles, varargin)




% --------------------------------------------------------------------
function varargout = pitchfcy_Callback(h, eventdata, handles, varargin)




% % --------------------------------------------------------------------
% function varargout = checkbox6_Callback(h, eventdata, handles, varargin)

% --------------------------------------------------------------------
function varargout = roll_Callback(h, eventdata, handles, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
value=get(handles.roll,'Value');
if(value==1)
    %set(handles.pitch,'Enable', 'on');
    %set(handles.roll,'Enable', 'off');
    %set(handles.yaw,'Enable', 'off');
%     
     set(handles.rollvalue,'Enable','on');
     %set(handles.rollvalue,'Enable','off');
     %set(handles.yawvalue,'Enable','off');
%     
     set(handles.rollfcy,'Enable','on');
     %set(handles.rollfcy,'Enable','off');
     %set(handles.yawfcy,'Enable','off');
%     
     set(handles.rollorigphase,'Enable','on');
     %set(handles.rollorigphase,'Enable','off');
     %set(handles.yaworigphase,'Enable','off');
     
    
 else
    % set(handles.pitch,'Enable', 'off');
    %set(handles.roll,'Enable', 'on');
    %set(handles.yaw,'Enable', 'on');
%     
     set(handles.rollvalue,'Enable','off');
     %$set(handles.rollvalue,'Enable','on');
     %set(handles.yawvalue,'Enable','on');
%     
     set(handles.rollfcy,'Enable','off');
     %set(handles.rollfcy,'Enable','on');
     %set(handles.yawfcy,'Enable','on');
%     
     set(handles.rollorigphase,'Enable','off');
     %set(handles.rollorigphase,'Enable','on');
     %set(handles.yaworigphase,'Enable','on');
     
     %%%%2003.12.4
     set(handles.rollvalue,'String','0');
     set(handles.rollfcy,'String','0');
     set(handles.rollorigphase,'String','0');
     %%%%%%%%%%%%
 end



% --------------------------------------------------------------------
function varargout = rollvalue_Callback(h, eventdata, handles, varargin)




% --------------------------------------------------------------------
function varargout = rollfcy_Callback(h, eventdata, handles, varargin)




% --------------------------------------------------------------------
function varargout = yaw_Callback(h, eventdata, handles, varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
value=get(handles.yaw,'Value');
if(value==1)
    %set(handles.pitch,'Enable', 'on');
    %set(handles.roll,'Enable', 'off');
    %set(handles.yaw,'Enable', 'off');
%     
     set(handles.yawvalue,'Enable','on');
     %set(handles.rollvalue,'Enable','off');
     %set(handles.yawvalue,'Enable','off');
%     
     set(handles.yawfcy,'Enable','on');
     %set(handles.rollfcy,'Enable','off');
     %set(handles.yawfcy,'Enable','off');
%     
     set(handles.yaworigphase,'Enable','on');
     %set(handles.rollorigphase,'Enable','off');
     %set(handles.yaworigphase,'Enable','off');
 else
    % set(handles.pitch,'Enable', 'off');
    %set(handles.roll,'Enable', 'on');
    %set(handles.yaw,'Enable', 'on');
%     
     set(handles.yawvalue,'Enable','off');
     %$set(handles.rollvalue,'Enable','on');
     %set(handles.yawvalue,'Enable','on');
%     
     set(handles.yawfcy,'Enable','off');
     %set(handles.rollfcy,'Enable','on');
     %set(handles.yawfcy,'Enable','on');
%     
     set(handles.yaworigphase,'Enable','off');
     %set(handles.rollorigphase,'Enable','on');
     %set(handles.yaworigphase,'Enable','on');
     
     %%%%2003.12.4
     set(handles.yawvalue,'String','0');
     set(handles.yawfcy,'String','0');
     set(handles.yaworigphase,'String','0');
     %%%%%%%%%%%%
 end


% --------------------------------------------------------------------
function varargout = yawvalue_Callback(h, eventdata, handles, varargin)




% --------------------------------------------------------------------
function varargout = yawfcy_Callback(h, eventdata, handles, varargin)




% --------------------------------------------------------------------
function varargout = pitchorigphase_Callback(h, eventdata, handles, varargin)




% --------------------------------------------------------------------
function varargout = rollorigphase_Callback(h, eventdata, handles, varargin)




% --------------------------------------------------------------------
function varargout = yaworigphase_Callback(h, eventdata, handles, varargin)




% --------------------------------------------------------------------
function varargout = sshapenorthspeed_Callback(h, eventdata, handles, varargin)




% --------------------------------------------------------------------
function varargout = sshapenorthspeedvalue_Callback(h, eventdata, handles, varargin)




% --------------------------------------------------------------------
function varargout = sshapeyaw_Callback(h, eventdata, handles, varargin)




% --------------------------------------------------------------------
function varargout = sshapeyawvalue_Callback(h, eventdata, handles, varargin)




% --------------------------------------------------------------------
function varargout = sshapefcy_Callback(h, eventdata, handles, varargin)




% --------------------------------------------------------------------
function varargout = sshapeorigphase_Callback(h, eventdata, handles, varargin)




% --------------------------------------------------------------------
function varargout = circleshapevalue_Callback(h, eventdata, handles, varargin)






% --------------------------------------------------------------------
function varargout = datageneration_Callback(h, eventdata, handles, varargin)

%%%%%%%%%%%%%%初始化参数为零；
ve=0;
vn=0;

ae=0;
an=0;

r_m=0;
p_m=0;
y_m=0;

omiga_r=0;
omiga_p=0;
omiga_y=0;

fai_r=0;
fai_p=0;
fai_y=0;


simutime=get(handles.simutime,'String');
mg=get(handles.mgyrodrift,'String');
ma=get(handles.maccebias,'String');
sg=get(handles.sgyrodrift,'String');
sa=get(handles.saccebias,'String');

simutime=str2num(simutime);
mg=str2num(mg);
ma=str2num(ma);
sg=str2num(sg);
sa=str2num(sa);


v1=get(handles.evenspeed,'value');
v2=get(handles.accespeed,'value');
v3=get(handles.sway,'value');
v4=get(handles.sshape,'value');
v5=get(handles.circleshape,'value');
v6=get(handles.rest,'value');


if  v1==1
    manner=1;
elseif v2==1
    manner=2;
elseif  v3==1
    manner=3;
elseif v4==1
    manner=4;
elseif v5==1
    manner=5;
elseif v6==1
    manner=6;
else 
    manner=0;
end

switch manner
case 1,
    ve=get(handles.eastspeedvalue,'String');
    ve=str2num(ve);
    
    vn=get(handles.northspeedvalue,'String');
    vn=str2num(vn);
    
case 2,
    ae=get(handles.eastaccervalue,'String');
    ae=str2num(ae);
    an=get(handles.northaccervalue,'String');
    an=str2num(an);
case 3,
    p_m=get(handles.pitchvalue,'String');
    p_m=str2num(p_m);
    
    r_m=get(handles.rollvalue,'String');
    r_m=str2num(r_m);
    
    y_m=get(handles.yawvalue,'String');
    y_m=str2num(y_m);
    
    
    omiga_p=get(handles.pitchfcy,'String');
    omiga_p=str2num(omiga_p);
    
    omiga_r=get(handles.rollfcy,'String');
    omiga_r=str2num(omiga_r);
    
    omiga_y=get(handles.yawfcy,'String');
    omiga_y=str2num(omiga_y);
    
    
    fai_p=get(handles.pitchorigphase,'String');
    fai_p=str2num(fai_p);
    
    fai_r=get(handles.rollorigphase,'String');
    fai_r=str2num(fai_r);
    
    fai_y=get(handles.yaworigphase,'String');
    fai_y=str2num(fai_y);
case 4,
    vn=get(handles.northspeedvalue,'String');
    vn=str2num(vn);
    
    y_m=get(handles.sshapeyawvalue,'String');
    y_m=str2num(y_m);
    
    omiga_y=get(handles.sshapeyawfcy,'String');
    omiga_y=str2num(omiga_y);
    
    fai_y=get(handles.sshapeyaworigphase,'String');
    fai_y=str2num(fai_y);
    
%     temp=simutime*10;
%     tt=[0:temp];
%     
     an=0.1;
     ae=0.12;
% 

case 5,
    omiga_y=get(handles.circleshapevalue,'String');
    omiga_y=str2num(omiga_y);
    omiga_y=omiga_y/360;
    %%%%
    y_m=10;
    
case 6,
    %%%%%%%%%
    y_m=1.2;
    p_m=1.5;
    r_m=2;
    
    omiga_y=0.1;
    omiga_p=0.12;
    omiga_r=0.18;
    
    fai_y=0;
    fai_p=0;
    fai_r=0;
   
end

nav_data_m1017(simutime, mg,ma, ve,vn, ae,an, y_m,p_m,r_m, omiga_y,omiga_p,omiga_r, fai_y,fai_p,fai_r,manner);
nav_data_s1017(sg,sa);

set(handles.smatch,'Enable','on');
set(handles.samatch,'Enable','on');
set(handles.sarmatch,'Enable','on');
set(handles.spmatch,'Enable','on');
set(handles.armatch,'Enable','on');
set(handles.amatch,'Enable','on');