- 001
- 002
- 003
- 004
- 005
- 006
- 007
- 008
- 009
- 010
- 011
- 012
- 013
- 014
- 015
- 016
- 017
- 018
- 019
- 020
- 021
- 022
- 023
- 024
- 025
- 026
- 027
- 028
- 029
- 030
- 031
- 032
- 033
- 034
- 035
- 036
- 037
- 038
- 039
- 040
- 041
- 042
- 043
- 044
- 045
- 046
- 047
- 048
- 049
- 050
- 051
- 052
- 053
- 054
- 055
- 056
- 057
- 058
- 059
- 060
- 061
- 062
- 063
- 064
- 065
- 066
- 067
- 068
- 069
- 070
- 071
- 072
- 073
- 074
- 075
- 076
- 077
- 078
- 079
- 080
- 081
- 082
- 083
- 084
- 085
- 086
- 087
- 088
- 089
- 090
- 091
- 092
- 093
- 094
- 095
- 096
- 097
- 098
- 099
- 100
// Begin Init Filter
void initFilter(){
int i,j;
double Pgam,Ptet,Ppsi,
sin_psi,cos_psi,sin_gam,cos_gam,cos_tet,
Pl,Pb,uu[81],bb[81]; //
hfo=1.; Nsf=1;
sugl[0]=1.5*M_PI/180.; sugl[1]=0.1*M_PI/180;sugl[2]=sugl[1];
dbc=100.; dbc*=0.157236869549e-7; dlc=dbc; dac=sugl[0];
Rk[0]=Omz/100000000.*hfo; Rk[1]=Omz/100000000.*hfo; Rk[2]=Rk[0];
Rk[3]=Omz/100000000.*hfo; Rk[4]=Rk[3]; Rk[5]=Rk[3];
for(i=0;i<3;i++) {
DTet[i]=0.;
Xs[i]=0.;
}
for(i=0;i<3;i++) {
som[i]=0.1*M_PI/(180.0*3600.0); // som[i]=3.*M_PI/(180.0*3600.0);
}
som[0]=0.1*M_PI/(180.0*3600.0); //som[0]=2.*M_PI/(180.0*3600.0);
Tauom[0]=360.; // Tauom[0]=300.;
Tauom[1]=360.; // Tauom[1]=100.;
Tauom[2]=360.; // Tauom[2]=300.;
for(i=0;i<9;i++) {
for(j=0;j<9;j++) {
Fko[i][j]=0.;
Fo[i][j]=0.;
Pko[i][j]=0.;
}
Xko[i]=0.;
Fko[i][i]=1.;
Fo[i][i]=1.;
}
for(i=0;i<6;i++) {
for(j=0;j<9;j++)
Hizo[i][j]=0.;
}
for(i=0;i<9;i++) {
for(j=0;j<3;j++)
Gko[i][j]=0.;
}
//begin SKO for MNK
Pgam=sugl[2]*sugl[2]; Ptet=sugl[1]*sugl[1]; Ppsi=sugl[0]*sugl[0];
Pl=dlc*dlc; Pb=dbc*dbc;
sin_psi=sin(Angl[0]); cos_psi=cos(Angl[0]);
sin_gam=sin(Angl[2]); cos_gam=cos(Angl[2]);
cos_tet=cos(Angl[1]);
uu[0]=Ur[0][0]*Ur[0][0]; bb[0]=B3[0][0]*B3[0][0];
uu[1]=Ur[0][0]*Ur[0][1]; bb[1]=B3[0][0]*B3[0][1];
uu[2]=Ur[0][0]*Ur[0][2]; bb[2]=B3[0][0]*B3[0][2];
uu[3]=Ur[0][0]*Ur[1][0]; bb[3]=B3[0][0]*B3[1][0];
uu[4]=Ur[0][0]*Ur[1][1]; bb[4]=B3[0][0]*B3[1][1];
uu[5]=Ur[0][0]*Ur[1][2]; bb[5]=B3[0][0]*B3[1][2];
uu[6]=Ur[0][0]*Ur[2][0]; bb[6]=B3[0][0]*B3[2][0];
uu[7]=Ur[0][0]*Ur[2][1]; bb[7]=B3[0][0]*B3[2][1];
uu[8]=Ur[0][0]*Ur[2][2]; bb[8]=B3[0][0]*B3[2][2];
uu[9]=Ur[0][1]*Ur[0][1]; bb[9]=B3[0][1]*B3[0][1];
uu[10]=Ur[0][1]*Ur[0][2]; bb[10]=B3[0][1]*B3[0][2];
uu[11]=Ur[0][1]*Ur[1][0]; bb[11]=B3[0][1]*B3[1][0];
uu[12]=Ur[0][1]*Ur[1][1]; bb[12]=B3[0][1]*B3[1][1];
uu[13]=Ur[0][1]*Ur[1][2]; bb[13]=B3[0][1]*B3[1][2];
uu[14]=Ur[0][1]*Ur[2][0]; bb[14]=B3[0][1]*B3[2][0];
uu[15]=Ur[0][1]*Ur[2][1]; bb[15]=B3[0][1]*B3[2][1];
uu[16]=Ur[0][1]*Ur[2][2]; bb[16]=B3[0][1]*B3[2][2];
uu[17]=Ur[0][2]*Ur[0][2]; bb[17]=B3[0][2]*B3[0][2];
uu[18]=Ur[0][2]*Ur[1][0]; bb[18]=B3[0][2]*B3[1][0];
uu[19]=Ur[0][2]*Ur[1][1]; bb[19]=B3[0][2]*B3[1][1];
uu[20]=Ur[0][2]*Ur[1][2]; bb[20]=B3[0][2]*B3[1][2];
uu[21]=Ur[0][2]*Ur[2][0]; bb[21]=B3[0][2]*B3[2][0];
uu[22]=Ur[0][2]*Ur[2][1]; bb[22]=B3[0][2]*B3[2][1];
uu[23]=Ur[0][2]*Ur[2][2]; bb[23]=B3[0][2]*B3[2][2];
uu[24]=Ur[1][0]*Ur[1][0]; bb[24]=B3[1][0]*B3[1][0];
uu[25]=Ur[1][0]*Ur[1][1]; bb[25]=B3[1][0]*B3[1][1];
uu[26]=Ur[1][0]*Ur[1][2]; bb[26]=B3[1][0]*B3[1][2];
uu[27]=Ur[1][0]*Ur[2][0]; bb[27]=B3[1][0]*B3[2][0];
uu[28]=Ur[1][0]*Ur[2][1]; bb[28]=B3[1][0]*B3[2][1];
uu[29]=Ur[1][0]*Ur[2][2]; bb[29]=B3[1][0]*B3[2][2];
uu[30]=Ur[1][1]*Ur[1][1]; bb[30]=B3[1][1]*B3[1][1];
uu[31]=Ur[1][1]*Ur[1][2]; bb[31]=B3[1][1]*B3[1][2];
uu[32]=Ur[1][1]*Ur[2][0]; bb[32]=B3[1][1]*B3[2][0];
uu[33]=Ur[1][1]*Ur[2][1]; bb[33]=B3[1][1]*B3[2][1];
uu[34]=Ur[1][1]*Ur[2][2]; bb[34]=B3[1][1]*B3[2][2];
uu[35]=Ur[1][2]*Ur[1][2]; bb[35]=B3[1][2]*B3[1][2];
uu[36]=Ur[1][2]*Ur[2][0]; bb[36]=B3[1][2]*B3[2][0];
uu[37]=Ur[1][2]*Ur[2][1]; bb[37]=B3[1][2]*B3[2][1];
uu[38]=Ur[1][2]*Ur[2][2]; bb[38]=B3[1][2]*B3[2][2];
uu[39]=Ur[2][0]*Ur[2][0]; bb[39]=B3[2][0]*B3[2][0];
uu[40]=Ur[2][0]*Ur[2][1]; bb[40]=B3[2][0]*B3[2][1];
uu[41]=Ur[2][0]*Ur[2][2]; bb[41]=B3[2][0]*B3[2][2];
и так далее