%陀螺和加速度计数据的产生
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------------------------------说明------------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------------------------------清除------------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear all;
tic; %开始计算程序运行时间 %启动秒表计时器——matlab库函数
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%------------------------------常用参数设置--------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Re=6378393; % 地球半径,m
wie=7.292115147e-5; % 地球自转角速度,rad/s
g0=9.7803267714; % 赤道处地球重力加速度
%常用的单位转化
D2R=pi/180; % 度-->弧度
R2D=180/pi; % 弧度-->度
R2S=180/pi; % 弧度-->度
DH2RS=pi/180/3600; % 度/h-->rad/s
%数据采样周期
T=0.05;
%Cbn=Angle2Cbn(yaw,roll,pitch); %此处调用了一个自定义的计算捷联矩阵的函数(可以认为是粗对准)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-------------------------------参数修改区---------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%载体初始位置经纬度
Longitude0=110*D2R; %经度,rad/s %D2R度到弧度的转变
Latitude0=40*D2R; %纬度,rad/s %D2R度到弧度的转变
Height0=0; %高度,m
%采样时间
Time=3600*1;
%数据个数(采样次数)
Number=Time/T;
%载体的基础姿态角,即载体在此基础上设置摇摆,设置为0度,不考虑z轴方向(航向角)的摇摆
yaw_base=0*D2R; %俯仰
roll_base=0*D2R; %横滚(倾斜角)
%载体的初始航向
psi0=30*D2R; %采用东北天坐标
%载体摇摆幅值(swing amplitude)
yaw_Am=3*D2R; %俯仰
roll_Am=5*D2R; %横滚(倾斜角)
%载体摇摆周期(swing cycle)
T_yaw=10; %俯仰,周期10秒
T_roll=10; %横滚(倾斜角),周期10秒
%载体摇摆初始相位
roll_0=30*D2R;
yaw_0=30*D2R;
%载体当前位置经纬高度,初值设为与设定相等
Longitude=Longitude0; %经度,rad/s %D2R度到弧度的转变
Latitude=Latitude0; %纬度,rad/s %D2R度到弧度的转变
Height=Height0; %高度,m
%载体当前姿态
yaw_now=yaw_base;
roll_now=roll_base;
psi_now=psi0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-----------------------------轨迹设置修改区-------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%匀速直线运动
psi_now=psi0; %直线运动航向一直不变
V=[10
10
0];
a=[0
0
0];
%;
%匀加速直线运动
%
%
%匀速圆周运动
%
%
%匀加速圆周运动
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%------------------------------误差设置------------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%陀螺仪的误差=常值漂移(0.01°/h)+随机漂移(0.005°/h)
Epsilon_x=0.01*DH2RS+0.005*DH2RS*randn(1,Number);
Epsilon_y=0.01*DH2RS+0.005*DH2RS*randn(1,Number);
Epsilon_z=0.01*DH2RS+0.005*DH2RS*randn(1,Number);
%加速度计误差=常值零偏(100μg)+随机零偏(50μg)
Delta_ax=1e-4*g0+1e-5*randn(1,Number)*g0;
Delta_ay=1e-4*g0+1e-5*randn(1,Number)*g0;
Delta_az=1e-4*g0+1e-5*randn(1,Number)*g0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-----------------------------陀螺数据输出---------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:Number
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-------------------------------位置更新-----------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Latitude=Latitude+V(2)*T/Re;
Longitude=Longitude+V(1)*T/(Re*cos(Latitude));
% phi = phi+v(2)*Hn/Re; %位置更新,phi是平台坐标系中纬度,lamda是平台坐标系中的经度
% lamda= lamda+v(1)*Hn/(Re*cos(phi));
% wepp = [-v(2)/Re;
% v(1)/Re;
% v(1)*tan(phi)/Re];
wn_en=[-V(2)/Re
V(1)/Re
(V(1)/Re)*tan(Latitude)];
% wiep = [0; wie*cos(phi); wie*sin(phi)];
wn_ie=[0
wie*cos(Latitude)
wie*sin(Latitude)];
%重力加速度更新
g=g0*(1+0.00193185138639*(sin(Latitude))^2)/sqrt(1-0.00669437999013*(sin(Latitude))^2);
psi_now=psi0;
yaw_now=yaw_base+yaw_Am*sin(2*pi*i*T/T_yaw+yaw_0);
roll_now=roll_base+roll_Am*sin(2*pi*i*T/T_roll+roll_0);
psi_now_daoshu=0;
yaw_now_daoshu=(yaw_Am*2*pi*T/T_yaw)*cos(2*pi*T*i/T_yaw+yaw_0);
roll_now_daoshu=(roll_Am*2*pi*T/T_roll)*cos(2*pi*T*i/T_roll+roll_0);
%捷联矩阵
Cbn(1,1)=cos(roll_now)*cos(psi_now)+sin(roll_now)*sin(psi_now)*sin(yaw_now);
Cbn(1,2)=-cos(roll_now)*sin(psi_now)+sin(roll_now)*cos(psi_now)*sin(yaw_now);
Cbn(1,3)=-sin(roll_now)*cos(yaw_now);
Cbn(2,1)=sin(psi_now)*cos(yaw_now);
Cbn(2,2)=cos(psi_now)*cos(yaw_now);
Cbn(2,3)=sin(yaw_now);
Cbn(3,1)=sin(roll_now)*cos(psi_now)-cos(roll_now)*sin(psi_now)*sin(yaw_now);
Cbn(3,2)=-sin(roll_now)*sin(psi_now)-cos(roll_now)*cos(psi_now)*cos(yaw_now);
Cbn(3,3)=cos(roll_now)*cos(yaw_now);
% Cbn=[cos(roll_now)*cos(psi_now)+sin(roll_now)*sin(psi_now)*sin(yaw_now) -cos(roll_now)*sin(psi_now)+sin(roll_now)*cos(psi_now)*sin(yaw_now) -sin(roll_now)*cos(yaw_now)
% sin(psi_now)*cos(yaw_now) cos(psi_now)*cos(yaw_now) sin(yaw_now)
% sin(roll_now)*cos(psi_now)-cos(roll_now)*sin(psi_now)*sin(yaw_now) -sin(roll_now)*sin(psi_now)-cos(roll_now)*cos(psi_now)*cos(yaw_now) cos(roll_now)*cos(yaw_now)];
Cnb=Cbn';
wb_nb(1,1)=cos(yaw_now)*sin(roll_now)*psi_now_daoshu+cos(roll_now)*yaw_now_daoshu;
wb_nb(2,1)=-sin(yaw_now)*psi_now_daoshu+roll_now_daoshu;
wb_nb(3,1)=-cos(yaw_now)*cos(roll_now)*psi_now_daoshu+sin(roll_now)*yaw_now_daoshu;
wn_en=[-V(2)/Re;
V(1)/Re;
(V(1)/Re)*tan(Latitude)];
wn_ie=[0;
wie*cos(Latitude);
wie*sin(Latitude)];
wb_ib=wb_nb+Cbn*(wn_en+wn_ie);
%带误差的陀螺的输出
wx(i)=wb_ib(1)+Epsilon_x(i);
wy(i)=wb_ib(2)+Epsilon_y(i);
wz(i)=wb_ib(3)+Epsilon_z(i);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%---------------------------------加速度计数据输出-------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fn=a+(2*wn_ie+wn_en)*V+[0;
% 0;
% g];
g = g0+0.051799*sin(Latitude)*sin(Latitude);
fn(1,1) = a(1) - (2*wn_ie(3)+wn_en(3))*V(2);
fn(2,1) = a(2) + (2*wn_ie(3)+wn_en(3))*V(1);
fn(3,1) = a(3) + (2*wn_ie(1)+wn_en(1))*V(2) - (2*wn_ie(2)+wn_en(2))*V(1) + g;
fb=Cbn*fn;
%考虑了所有误差后的加速度计表的输出
fbx(i)=fb(1)+Delta_ax(i); %x方向比力+加速度计零偏,fbx为列向量
fby(i)=fb(2)+Delta_ay(i);
fbz(i)=fb(3)+Delta_az(i);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%------------------------------保存数据------------------------------------%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%此文件夹中有一个txt文档,名为‘d1’,用来存放数据。
for