%function?[accelerometer?gyro]=acc_gyro（trajectory_typecaiyangTtotaltime）
%%生成陀螺儀加速度計(jì)的輸出數(shù)據(jù)
%與trajectory配合使用
%讀取真實(shí)數(shù)據(jù)
clear?all;
clc;
close?all;
trajectory_type=1;
caiyangT=0.1;
totaltime=600;

if?trajectory_type==1;
????load?Storedata_line?;
elseif?trajectory_type==2
????load?Storedata_line2;
elseif?trajectory_type==3
????load?Storedata_circle;
end
%?第一行表示真實(shí)軌跡類型，采樣時(shí)間等參數(shù)
%?軌跡的類型：%?1.勻速直線運(yùn)動(dòng)??%?2.加速直線運(yùn)動(dòng)??%?3.勻速圓周運(yùn)動(dòng)
??
%?采樣的時(shí)間?caiyangT
%?采樣的點(diǎn)數(shù)?step
%?仿真的時(shí)間?totaltime
parameter?=?Storedata（11:3）;
true_type?=?parameter（1）;
true_caiyangT?=?parameter（2）;
true_step?=?parameter（3）;
if（trajectory_type~=true_type）
????error（‘軌跡類型不匹配‘）;
end
if（caiyangT????fprintf（‘真實(shí)數(shù)據(jù)采樣時(shí)間為：‘）;disp（true_caiyangT）;
????error（‘采樣時(shí)間時(shí)間應(yīng)小于真實(shí)真實(shí)采樣時(shí)間‘）;
end
if（totaltime>true_caiyangT*true_step）
????error（‘仿真時(shí)間超過真實(shí)數(shù)據(jù)仿真時(shí)間‘）;
end
%???捷聯(lián)慣導(dǎo)系統(tǒng)需要飛行軌跡產(chǎn)生的五組十五個(gè)軌跡數(shù)據(jù)
%???1~3列位置：緯度，經(jīng)度，高度:Storedata_l?緯度經(jīng)度以弧度為單位
%???4~6列?地理系下載體相對于地球系的速度:Storedata_v?東向，北向，天向
%???7~9列地理系下載體相對于地球系的加速度?:Storedata_a?東向，北向，天向
%???10~12列姿態(tài)角：俯仰角、橫滾角、航向角?:Storedata_e?
%???13~15列?機(jī)體下的坐標(biāo)系相對于地理系的轉(zhuǎn)動(dòng)角速率:Storedata_w?
[row?line]=?size（Storedata）;%得到Storedata的行數(shù)和列數(shù)
Storedata1?=?Storedata（2:row1:15）;%第一行為Storedata的特征量舍去
Location?=?Storedata1（1:floor（caiyangT/true_caiyangT）:true_step1:3）;%floor（x）：向左取整
Velocity?=?Storedata1（1:floor（caiyangT/true_caiyangT）:true_step4:6）;
Acceleration?=?Storedata1（1:floor（caiyangT/true_caiyangT）:true_step7:9）;
Attitude?=?Storedata1（1:floor（caiyangT/true_caiyangT）:true_step10:12）;

Angle_ve?=?Storedata1（1:floor（caiyangT/true_caiyangT）:true_step13:15）;
%%
%生成加速度計(jì)，陀螺儀數(shù)據(jù)

global?Ra?e?wie;
Ra?=?6378140;%6000000%地球參考橢球長軸半徑
e?=?1/298.257;%0%地球橢球的橢圓度
wie?=?15.04088/3600/180*pi;?%0%%地球自轉(zhuǎn)角速度
hWaitBar?=?waitbar（0‘Please?wait...‘）;

fprintf（‘正在生成加速度計(jì)陀螺儀數(shù)據(jù)……\n‘）;
simT?=?[0:caiyangT:totaltime]‘;%仿真的時(shí)間
step?=size（simT1）;%采樣的點(diǎn)數(shù)
accelerometer1=zeros（step3）;
gyro1=zeros（step3）;
for?i=1:step????
????Ve?=?Velocity（i1）;%東向的速度
????Vn?=?Velocity（i2）;%北向的速度
????Vu?=?Velocity（i3）;%天向的速度
????latitude?=?Location（i1）;%緯度
????RN?=?Ra*（1-2*e+3*e*sin（latitude）^2）;%參考橢球子午面內(nèi)的曲率半徑，沿南北方向
????RE?=?Ra*（1+e*sin（latitude）^2）;%參考橢球子午面的法線平面內(nèi)的曲率半徑，沿東西方向
????g?=?9.7803267714*（1+0.00193185138639*sin（latitude）^2）/sqrt（1-0.00669437999013*sin（latitude）^2）;
????gn?=?[0?0?-g]‘;%地理系上的重力
????dot_latitude?=?Vn/RN;
????dot_longitude?=?Ve/（RE*cos（latitude））;
????%俯仰角定義為載體縱軸與縱向水平軸之間的夾角，規(guī)定以縱向水平軸為起點(diǎn)，向上為正，向下為負(fù)，定義域?yàn)?90°~+90°
????%橫滾角定義為載體縱向?qū)ΨQ面與縱向鉛垂面之間的夾角，規(guī)定從鉛垂面算起，右傾為正，左傾為負(fù)，定義域?yàn)?180°~+180°
????%航向角定義為載體縱軸在水平面的投影與地理子午線之間的夾角，規(guī)定以地理北向?yàn)槠瘘c(diǎn)，偏東方向?yàn)檎x域?yàn)?~360°
????theta?=?Attitude（i1）;%俯仰角
????gama?=?Attitude（i2）;%滾動(dòng)角
????fai?=?Attitude（i3）;%航向角
????%導(dǎo)航坐標(biāo)系（地理坐標(biāo)系）到體坐標(biāo)系的坐標(biāo)變換矩陣
????%%%？？？？？？？？？元素對嗎，cbn還是cnb？？？？
????%%%這一下原Cb_n被我替換成Cn_b不知道對其他子程序有沒有影響
????%????????Cb_n?=?[sin（fai）*sin（theta）*sin（gama）+cos（fai）*cos（gama）???cos（fai）*sin（theta）*sin（gama）-sin（fai）*cos（gama）???-cos（theta）*sin（gama）;
????%????????sin（fai）*cos（theta）??????????????????????????????

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2017-04-28?09:30??整套程序\
?????文件????????7516??2017-04-20?09:14??整套程序\acc_gyro.m
?????文件??????188372??2019-12-01?19:32??整套程序\acc_gyro_line.mat
?????文件???????91008??2019-12-01?19:32??整套程序\acc_line_fb.mat
?????文件????????7589??2019-12-01?19:35??整套程序\GPSdata_line.mat
?????文件?????????200??2019-12-01?19:35??整套程序\GPSdata_location0.mat
?????文件????????7743??2017-04-20?11:33??整套程序\guandaochengxucopy2.m
?????文件???????97329??2019-12-01?19:32??整套程序\gyro_line_wibb.mat
?????文件???????65716??2019-12-01?19:35??整套程序\Storedata_line.mat
?????文件????????8949??2017-04-20?11:29??整套程序\trajectory.m
?????文件?????????292??2017-04-28?09:30??整套程序\各文件信息.txt
?????文件?????????270??2017-04-20?09:22??整套程序\各文件信息.txt.bak