clear;
clc;
sampletime=0.01; %采样时间；姿态更新周期
Time=1000;
number=Time/sampletime; %采样总数=100,000
PI=3.14159265358979323846;
e=1/298.257; %地球扁率
g=9.8; %重力加速度
gn=[0;0;-g]; %导航坐标系
Re=6378140; %地球的半径
Wie=7.292115e-5; %地球自转角速度
dh=PI/180; %弧度转换成度
hd=180/PI; %度转换成弧度
dh_hs=PI/180.0/3600.0; %度每小时到弧度每秒的转化
hs_dh=180*3600/PI;
%V=[0;0;0]; %速度初始化，分别为东向、北向、天向速度
Velo=10.0; %初始速度设置为0
EVelo=0.01;
H0=0*dh; %初始方位角
P0=0*dh;
R0=0*dh;
L0=32*dh; %纬度
longi0=118.0*dh; %经度
EL=0.0001*dh; %初始经度、纬度误差
Elongi=0.0001*dh;
A_H=0.0*dh; %正弦波的振幅
A_P=0.0*dh;
A_R=0.0*dh;
W_H=2.0*PI/6.0; %正弦波的频率
W_P=2.0*PI/8.0; %正弦波的频率
W_R=2.0*PI/10.0; %正弦波的频率
gyrocon=0.02;
gyroran=0.05;
gyrotrend=0;
gyrotrendphase=0*dh;
gyrotrendT=2*PI/9000;
accecon=0.1;
acceran=0.05;
ERRH=5*dh;%转位机构误差
ERRP=0.1*dh;%调平机构误差
ERRR=0.1*dh;
RK=1;
QK=1;
PK=10.0;
count=0;
f0=fopen('error_13.dat','w');
f1=fopen('x_13.dat','w');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%初始化
tH=H0;%理论的姿态角初值
tP=P0;
tR=R0;
H=H0+ERRH;%实际的姿态角初值
P=P0+ERRP;
R=R0+ERRR;
L=L0+EL;
longi=longi0+Elongi;
% L=L0;
tL=L0;
% longi=longi0;
tlongi=longi0;
RM=Re*(1-2*e+3*e*sin(L0)*sin(L0)); %沿子午圈的曲率半径实际
RN=Re*(1+e*sin(L0)*sin(L0)); %沿卯酉圈的曲率半径
tRM=Re*(1-2*e+3*e*sin(L0)*sin(L0)); %沿子午圈的曲率半径理论
tRN=Re*(1+e*sin(L0)*sin(L0)); %沿卯酉圈的曲率半径
tV(1,1)=-Velo*sin(tH);%理论速度定义在水平方向上的运动，天向速度为0
tV(2,1)=Velo*cos(tH);
tV(3,1)=0.0;
V(1,1)=-(Velo+EVelo)*sin(H);%实际速度赋初值
V(2,1)=(Velo+EVelo)*cos(H);
% V(1,1)=-Velo*sin(H);%实际速度赋初值
% V(2,1)=Velo*cos(H);
V(3,1)=0.0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%计算理论姿态矩阵
tCbn=[cos(tR)*cos(tH)-sin(tR)*sin(tP)*sin(tH), cos(tR)*sin(tH)+sin(tP)*sin(tR)*cos(tH), -sin(tR)*cos(tP);
-cos(tP)*sin(tH), cos(tP)*cos(tH), sin(tP);
sin(tR)*cos(tH)+cos(tR)*sin(tP)*sin(tH), sin(tR)*sin(tH)-cos(tR)*sin(tP)*cos(tH), cos(tR)*cos(tP)];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%计算实际姿态矩阵
Cbn=[cos(R)*cos(H)-sin(R)*sin(P)*sin(H), cos(R)*sin(H)+sin(P)*sin(R)*cos(H), -sin(R)*cos(P);
-cos(P)*sin(H), cos(P)*cos(H), sin(P);
sin(R)*cos(H)+cos(R)*sin(P)*sin(H), sin(R)*sin(H)-cos(R)*sin(P)*cos(H), cos(R)*cos(P)];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%用欧拉角初始化四元数
Q0(1,1)=cos(H/2)*cos(P/2)*cos(R/2)-sin(H/2)*sin(P/2)*sin(R/2);%四元数初始化
Q0(2,1)=cos(H/2)*sin(P/2)*cos(R/2)-sin(H/2)*cos(P/2)*sin(R/2);
Q0(3,1)=cos(H/2)*cos(P/2)*sin(R/2)+sin(H/2)*sin(P/2)*cos(R/2);
Q0(4,1)=sin(H/2)*cos(P/2)*cos(R/2)+cos(H/2)*sin(P/2)*sin(R/2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%对滤波状态进行初始化
x=zeros(12,1);%%%%初始化滤波估计值
xk=zeros(12,1);%%%%初始一步预测估计值
shuzu0=zeros(12,12);
shuzu1=zeros(12,12);
cm_gg=zeros(12,12);
gg=zeros(12,12);
cpk=zeros(12,12);%%%%初始一步预测估计误差的方差值
cp=zeros(12,12);%%%初始滤波估计误差的方差阵？？？如何计算的
cp(1,1)=PK*(0.01^2);
cp(2,2)=PK*(0.01^2);
cp(3,3)=PK*((0.1*dh)^2);
cp(4,4)=PK*((0.1*dh)^2);
cp(5,5)=PK*((5*dh)^2);
cp(6,6)=PK*((0.0001*dh)^2);
cp(7,7)=PK*((0.0001*dh)^2);
cp(8,8)=PK*((5e-5*9.8)^2);
cp(9,9)=PK*((5e-5*9.8)^2);
cp(10,10)=PK*((0.02*dh_hs)^2);
cp(11,11)=PK*((0.02*dh_hs)^2);
cp(12,12)=PK*((0.02*dh_hs)^2);
qq=zeros(12,12);%%%初始系统噪声误差的协方差阵
qq(1,1)=QK*((1e-5*9.8)^2);
qq(2,2)=QK*((1e-5*9.8)^2);%%%加速度计噪声误差
qq(3,3)=QK*((0.01*dh_hs)^2);
qq(4,4)=QK*((0.01*dh_hs)^2);
qq(5,5)=QK*((0.01*dh_hs)^2);%%%%陀螺仪噪声误差
rr=zeros(7,7);
rr(1,1)=RK*(0.01^2);
rr(2,2)=RK*(0.01^2);%%%%%速度
rr(3,3)=RK*((0.01*dh)^2);
rr(4,4)=RK*((0.01*dh)^2);
rr(5,5)=RK*((0.01*dh)^2);%%%%姿态角
rr(6,6)=RK*((0.0001*dh)^2);
rr(7,7)=RK*((0.0001*dh)^2);%%%%%经纬度
zk=zeros(7,1);%%初始化量测矩阵
hh=zeros(7,12);%%%初始观测矩阵
Cnb=(Cbn)';
hh(3,3)=Cnb(1,2)*Cnb(3,2)/(Cnb(1,2)^2+Cnb(2,2)^2);
hh(3,4)=Cnb(2,2)*Cnb(3,2)/(Cnb(1,2)^2+Cnb(2,2)^2);
hh(3,5)=-1;
hh(4,3)=-Cnb(2,2)/sqrt(1-Cnb(3,2)^2);
hh(4,4)=Cnb(1,2)/sqrt(1-Cnb(3,2)^2);
hh(5,3)=(Cnb(2,1)*Cnb(3,3)-Cnb(2,3)*Cnb(3,1))/(Cnb(3,1)^2+Cnb(3,3)^2);
hh(5,4)=(Cnb(1,3)*Cnb(3,1)-Cnb(1,1)*Cnb(3,3))/(Cnb(3,1)^2+Cnb(3,3)^2);
hh(1,1)=1.0;
hh(2,2)=1.0;
hh(6,6)=1.0;
hh(7,7)=1.0;
kk=zeros(12,7);%%%初始增益矩阵
for j=1:number
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%白噪声以及陀螺误差
noise=0.001*randn(3,1);
Gyro_Err(1,1) = (gyrocon + gyroran*noise(1,1) + gyrotrend*sin(gyrotrendT*j*sampletime + gyrotrendphase))*dh_hs;%光纤陀螺噪声
Gyro_Err(2,1) = (gyrocon + gyroran*noise(2,1) + gyrotrend*sin(gyrotrendT*j*sampletime + gyrotrendphase))*dh_hs;
Gyro_Err(3,1) = (gyrocon + gyroran*noise(3,1) + gyrotrend*sin(gyrotrendT*j*sampletime + gyrotrendphase))*dh_hs;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%更新理论经纬度，姿态角，速度，东半径，北半径
tH=H0+A_H*sin(W_H*j*sampletime);%摇摆时的姿态更新，静基座时振幅为0
tP=P0+A_P*sin(W_P*j*sampletime);
tR=R0+A_R*sin(W_R*j*sampletime);
dtH=A_H*W_H*cos(W_H*j*sampletime); %求导
dtP=A_P*W_P*cos(W_P*j*sampletime);
dtR=A_R*W_R*cos(W_R*j*sampletime);
tL=tL+tV(2,1)*sampletime/tRM; %理论经纬度更新
tlongi=tlongi+tV(1,1)*sampletime/(tRN*cos(tL));
tRM=Re*(1-2*e+3*e*sin(tL)*sin(tL)); %沿子午圈的曲率半径
tRN=Re*(1+e*sin(tL)*sin(tL)); %沿卯酉圈的曲率半径
tV(1,1)=-Velo*sin(tH);%理论速度定义在水平方向上的运动，天向速度为0
tV(2,1)=Velo*cos(tH);
tV(3,1)=0.0;
dtV(1,1)=-Velo*cos(tH)*dtH;%理论速度求导
dtV(2,1)=-Velo*sin(tH)*dtH;
dtV(3,1)=0.0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%理论姿态矩阵更新
tCbn=[cos(tR)*cos(tH)-sin(tR)*sin(tP)*sin(tH), cos(tR)*sin(tH)+sin(tP)*sin(tR)*cos(tH), -sin(tR)*cos(tP);
-cos(tP)*sin(tH), cos(tP)*cos(tH), sin(tP);
sin(tR)*cos(tH)+cos(tR)*sin(tP)*sin(tH), sin(tR)*sin(tH)-cos(tR)*sin(tP)*cos(tH), cos(tR)*cos(tP)];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%陀螺仪仿真输出
tWnie=[0;Wie*cos(tL);Wie*sin(tL)];
tWnen=[-tV(2,1)/tRM;tV(1,1)/tRN;tV(1,1)*tan(tL)/tRN];
tWnin=tWnie+tWnen;
tWbnb(1,1)=-sin(tR)*cos(tP)*dtH+cos(tR)*dtP;
tWbnb(2,1)=sin(tP)*dtH+dtR;
tWbnb(3,1)=cos(tR)*cos(tP)*dtH+sin(tR)*dtP;
tWbib=tCbn*tWnin+tWbnb;%理论光纤陀螺输出
Wbib=tWbib+Gyro_Err; %实际光纤陀螺输出
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%计算真实的Wbnb，以便于四元数解算时使用
Wnie=[0;Wie*cos(L);Wie*sin(L)];
Wnen=[-V(2,1)/RM;V(1,1)/RN;V(1,1)*tan(L)/RN];
Wnin=Wnie+Wnen;
Cbn=[cos(R)*cos(H)-sin(R)*sin(P)*sin(H), cos(R)*sin(H)+sin(P)*sin(R)*cos(H), -sin(R)*cos(P);
-cos(P)*sin(H), cos(P)*cos(H), sin(P);
sin(R)*cos(H)+cos(R)*sin(P)*sin(H), sin(R)*sin(H)-cos(R)*sin(P)*cos(H), cos(R)*cos(P)];
%用带误差值的光纤陀螺输出-用误差数据算出来的Cbn*Wnin，一定得到的是误差数据
Wbnb=Wbib-Cbn*Wnin;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%四元数姿态解算
Wbnbx=Wbnb(1,1);
Wbnby=Wbnb(2,1);
Wbnbz=Wbnb(3,1);
GW=[0, -Wbnbx, -Wbnby, -Wbnbz;
Wbnbx, 0, Wbnbz, -Wbnby;
Wbnby, -Wbnbz, 0, Wbnbx;
Wbnbz, Wbnby, -Wbnbx, 0];
M=GW*sampletime;%采样时间间隔内的角增量
PP=sqrt(M(2,1)^2+M(3,1)^2+M(4,1)^2);
I=eye(4,4);%返回4*4的单位矩阵I
qua=(I*(1-PP*PP/8+(PP^4)/384)+(0.5-PP*PP/48)*M)*Q0;%%四阶近似算法这块先不改，看会不会有问题
%qua=(I*cos(P/2)+M*sin(P/2)/P)*Q0;%
评论1