基于MATLAB----静态情况陀螺输出仿真

参考了一下师兄的源代码，感觉还是学到很多的。

本篇代码从一开始先写陀螺和加速度漂移的初始化，然后写了采样条件，创建文件，陀螺输出值为Wib_b，加速度输出为F_b，

接下来就是循环，一共循环采样次数的大小数，循环过程中不断姿态更新，个人觉得核心的过程是：

想要求解Wib_b，要求Wib_b=Win_b+Wnb_b；

所以分开去求二者；

Win_b=Cn2b*Win_n；

Wnb_b的求解在程序中具体体现，比较繁琐，个人觉得这些不用理解，就是公式而已，会用就行。接下来就是把噪声加上去

至于加速度输出就很简单，乘以姿态矩阵即可。

总之，这应该是入门程序吧；

%。。。。。。。。。。。静基座下传感器输出仿真。。。。。。。。。。。。。。。。。。。。。。

%仿真条件：
%          （1）陀螺精度：0.1°/h-0.08°/h
%                        0.08°/h-0.05°/h
%                        0.05°/h-0.01°/h
%          （2）水平（10"）、四位置转位（10"）
%           (3)寻北时间：2min/5min/10min

function ChuanGanQi_Out

%%    大地固定参数

 global     PI Wie D_h dh_hs ;

  PI         = 3.14159265358979323846;         % 圆周率
  g          = 9.794843;                       % 重力加速度
  Wie        = 7.292115e-5;                    % 地球自转角速率
  D_h        = PI/180.0;                       % 度到弧度的单位转换  
%   H_d        = 180.0/PI;                       % 弧度到度的单位转换
  dh_hs      = PI/180.0/3600.0;                % 度每小时到弧度每秒的单位转换
  hs_dh      = 180.0*3600.0/PI;                % 弧度每秒到度每小时的单位转换

%%    初始化区

% 与姿态角相关的初始化

  global H0 P0 R0 A_H A_P A_R W_H W_P W_R
  H0 = 0.00*D_h;     % 初始航向     head
  P0 = 0.00*D_h;     % 初始纵摇     pitch
  R0 = 0.00*D_h;     % 初始横摇     roll

  A_H = 0.0*D_h;    % 正弦波的振幅
  A_P = 0.0*D_h;    % 正弦波的振幅
  A_R = 0.0*D_h;    % 正弦波的振幅

  W_H = 0.0*PI/5.0;  % 正弦波的频率
  W_P = 0.0*PI/5.0;  % 正弦波的频率
  W_R = 0.0*PI/5.0;  % 正弦波的频率
  
  Err_H = 10.0/3600*D_h;           % 转位机构误差
  Err_P = 10.0/3600*D_h;           % 调平机构误差
  Err_R = 10.0/3600*D_h;           % 调平机构误差
  
  % 初始纬度值
  
  tlatit =  32.0*D_h;
  
  % 初始速度
  
%   Velo = 0;

% 姿态矩阵初始化

  tCn2b = [1 0 0;0 1 0;0 0 1];
  
% 陀螺漂移初始化
  gyro_con  =  0.05 ;             % 陀螺常值漂移            度/h
  
  gyro_trend = 0.05;               % 陀螺的常值漂移趋势项     度/h
  gyro_trend_w = 2*PI/90;          % 陀螺的常值漂移趋势项周期 
  gyro_trendphase = 10.3*D_h;     % 趋势项的相位    

  % 加速度漂移初始化
  
  Acc_con = 0.02;                 % 加速度常值漂移
  
  % 采样条件

sampletime = 0.01;           % 姿态更新周期，单位为妙（s）
Time       = 120;            % 采用时间，单位为妙（s）
numble     = Time/sampletime;% 采样总数

%...............文件打开。。。。。。。。。。。。。。。。。。。。

[fid1,message1]=fopen('GyroScope_Out.txt','w');
if fid1==-1
    disp(message1);
end
[fid2,message1]=fopen('F_Out.txt','w');
if fid2==-1
    disp(message1);
end

%%    输出算法

Wib_b = zeros(3*numble,1);   % 陀螺输出初始值     numble = Time/sampletime;% 采样总数
tWib_b = zeros(3*numble,1);
D_gyro = zeros(3*numble,1);  % 初始陀螺漂移

F_b = zeros(3*numble,1);     % 加速度输出初始值
tFb = zeros(3*numble,1);
D_f = zeros(3*numble,1);     % 初始加速度漂移

for i = 1:numble
    
    tH = H0+A_H*sin(W_H*i*sampletime)+ Err_H; % 初始航向\正弦波的振幅\正弦波的频率\转位机构误差
	tP = P0+A_P*sin(W_P*i*sampletime)+ Err_P;     
	tR = R0+A_R*sin(W_R*i*sampletime)+ Err_R;
    
% 求导
	dtH = A_H*W_H*cos(W_H*i*sampletime);
	dtP = A_P*W_P*cos(W_P*i*sampletime);
	dtR = A_R*W_R*cos(W_R*i*sampletime);
  
    tWin_n(1) = 0;
	tWin_n(2) = Wie*cos(tlatit);
	tWin_n(3) = Wie*sin(tlatit);
    
    tCn2b(1,1) = cos(tR)*cos(tH)-sin(tR)*sin(tP)*sin(tH);
	tCn2b(1,2) = cos(tR)*sin(tH)+sin(tR)*sin(tP)*cos(tH);
	tCn2b(1,3) =-sin(tR)*cos(tP);
    tCn2b(2,1) = -cos(tP)*sin(tH);
	tCn2b(2,2) = cos(tP)*cos(tH);
	tCn2b(2,3) = sin(tP);
	tCn2b(3,1) = sin(tR)*cos(tH)+cos(tR)*sin(tP)*sin(tH);
	tCn2b(3,2) = sin(tR)*sin(tH)-cos(tR)*sin(tP)*cos(tH);
	tCn2b(3,3) = cos(tR)*cos(tP);
  
% 陀螺输出    
    tWin_b = tCn2b*tWin_n';     % 陀螺输出初始值
     
    tWnb_b(1) = -sin(tR)*cos(tP)*dtH+cos(tR)*dtP;
	tWnb_b(2) = sin(tP)*dtH+dtR;
	tWnb_b(3) = cos(tR)*cos(tP)*dtH+sin(tR)*dtP;
    
    tWib_b((3*(i-1)+1):(3*i)) = tWin_b+tWnb_b';
    
    noise =2*(rand(1)-0.5);              %  产生陀螺随机白噪声
    
    for j = (3*(i-1)+1):(3*i)
        D_gyro(j) = (0.2*noise+gyro_con+gyro_trend*sin(gyro_trend_w*j*sampletime+gyro_trendphase))*dh_hs;
    end

    Wib_b((3*(i-1)+1):(3*i)) = (tWib_b((3*(i-1)+1):(3*i))+D_gyro((3*(i-1)+1):(3*i)))*hs_dh;
    fprintf(fid1,'%g\t',Wib_b((3*(i-1)+1):(3*i)));  
    
    % 加速度输出
    
    noise_f =2*(rand(1)-0.5);              %  产生加速度随机白噪声(-1,1)
    g_n = [0 0 g];
    tFb((3*(i-1)+1):(3*i)) = tCn2b*g_n';
    for j = (3*(i-1)+1):(3*i)
        D_f(j) =  (0.2*noise_f+Acc_con )/g/1000;
    end
    F_b((3*(i-1)+1):(3*i)) = tFb((3*(i-1)+1):(3*i)) + D_f((3*(i-1)+1):(3*i));
    fprintf(fid2,'%g\t',F_b((3*(i-1)+1):(3*i)));  
end


%。。。。。。。。。 保存数据到文件夹。。。。。。。。。。

%  fprintf(fid1,'%g\t',Wib_b);  
% fprintf(fid1,'\n'); 


%%    输出仿真图

%..................加载陀螺输出文件..........................
Wib_b0  = load('GyroScope_Out.txt');
F_b0 = load('F_Out.txt');
figure

subplot(3,1,1);
plot(Wib_b0(1:3:3*numble-2));
title('东向陀螺输出');xlabel('t');ylabel('（°/h）');
subplot(3,1,2);
plot(Wib_b0(2:3:3*numble-1));
title('北向陀螺输出');xlabel('t');ylabel('（°/h）');
subplot(3,1,3);
plot(Wib_b0(3:3:3*numble));
title('方位陀螺输出');xlabel('t');ylabel('（°/h）');

%.................加载加速度输出文件..........................
figure

subplot(3,1,1);
plot(F_b0(1:3:3*numble-2));
title('东向加速度输出');xlabel('t');ylabel('（g）');
subplot(3,1,2);
plot(F_b0(2:3:3*numble-1));
title('北向加速度输出');xlabel('t');ylabel('（g）');
subplot(3,1,3);
plot(F_b0(3:3:3*numble));
title('方位加速度输出');xlabel('t');ylabel('（g）');