orbit.f
13.4 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
subroutine orbit (day,kode,lfile,nsat,x,revnum,ier)
c*
c***********************************************************************
c*
c* "Copyright [c] CNES 98 - tous droits reserves"
c* **********************************************
c*
c*PRO MAGLIB
c*
c*VER 99.03.31 - V 1.0
c*VER 01.06.05 - V 2.0
c*VER 03.01.06 - V 2.1
c*
c*AUT spec. ESA - janvier 1991
c*AUT port. CISI
c*
c*
c*ROL Theme : Astronomie et calcul d'orbite
c*ROL Calcul du vecteur d'etat a partir des donnees
c*ROL d'orbitographie de Cluster (compsants de Chebychev)
c*ROL par resolution de l'equation de Kepler.
c*ROL Un fichier de coefficients de Chebychev est necessaire.
c*
c*PAR day (I) : date julienne ref 2000 (a partir de 01/01/2000)
c*
c*PAR kode (I) : dimension du vecteur d'etat
c*
c*PAR lfile (I) : numero logique associe au fichier en entree
c*
c*PAR nsat (O) : identification du satellite 1, 2, 3 ou 4
c*
c*PAR x (O) : vecteur d'etat : position en x, y et z du satellite
c* : (kilometres) ; vitesse en x, y et z (kilometres/secondes)
c*
c*PAR revnum (O) : numero d'orbite
c*
c*PAR ier (O) : code de retour
c*
c*NOT kode : 3 = position
c*NOT kode : 6 = position et vitesse
c*
c*NOT ier : 0 = OK
c*NOT ier : 1 = date julienne erronee (trop petite)
c*NOT ier : 2 = date julienne erronee (trop grande)
c*NOT ier : 3 = non continuite de la date entre 2 enreg consecutifs
c*NOT ier : 4 = valeur de kode erronee (differente de 3 ou 6)
c*NOT ier : 5 = fichier incoherent (donnees invalides)
c*NOT ier : 6 = erreur de lecture
c*
c*NOT : Le fichier doit etre ouvert au prealable.
c*NOT : Les enregistrements doivent avoir des dates consecutives
c*NOT : (date debut enreg i - date de fin enreg i-1 > gap
c*NOT : avec gap = 2.d-4)
c*
c*NOT common : ssave
c*NOT : Common de sauvegarde des donnees remanentes.
c*
c*INF utilise : sans objet
c*
c*HST version 1.0 - 99.03.31 - creation de la maglib au CDPP
c*HST version 2.0 - 01.06.05 - correction de commentaires de code
c*HST version 2.1 - 03.01.06 - corrections en compilation avec g77
c*
c***********************************************************************
c*
implicit none
c
c ---------------------------------
c*FON Declaration identificateur rcs_id
c ---------------------------------
c
character rcs_id*100
c
c --------------------------
c*FON Declaration des parametres
c --------------------------
c
double precision day
integer kode,lfile,nsat
double precision x(kode),revnum
integer ier
c
c ----------------------------------
c*FON Declaration des variables communes
c ----------------------------------
c
c indicateur de lecture pour le 1er appel
integer mfile
c date debut et date fin de l'enreg courant
double precision daybeg,dayend
c date de debut du fichier - marge
c date de fin du bloc courant + marge
double precision dayfir,daylas
c
common/ssave/daybeg,dayend,dayfir,daylas,mfile
c
c ---------------------------------
c*FON Declaration des variables locales
c ---------------------------------
c
integer nrec
integer iter
integer k,i,j,l
c*LOC k,i,j,l : indices de boucles
c
double precision epoch
c*LOC epoch : date de reference du vecteur d'etat
c
double precision revepo
c*LOC revepo : numero d'orbite associe
c
double precision smaxis
c*LOC smaxis : demi grand axe associe en km
c
double precision omotin
c*LOC omotin : mouvement moyen inverse pour une orbite Keplerienne
c
double precision coeff(10,6)
c*LOC coeff : table des coefficients du polynome de Chebyshev pour les
c*LOC 6 composants du vecteur d'etat
c
integer koeff
c*LOC koeff : nombre de coefficients polynomiaux (0< koeff <10)
c
double precision y(6)
c*LOC y : vecteur d'etat
c
double precision rdist
c*LOC rdist : distance centre terre satellite a la date courante en km
c
double precision s
c*LOC s : coefficient poynomial de Chebyshev
c
double precision dmanom
double precision arin,arm,rvwam,tam,comp,b,go,g1,bet
double precision d,g2,g3,fx,gx,rx,ft,gt
double precision daymid
double precision scale
double precision pa,p,pb
c*LOC Variables de travail intermediaires
c
SAVE
c
c ---------------------------------
c*FON Affectation identificateur rcs_id
c ---------------------------------
c
data rcs_id /"
>$Id$"/
c
c ******************
c Debut de programme
c ******************
c
ier = 0
c
if (kode .le. 0) goto 902
if (kode .gt. 6) goto 902
c
c cas de la 1er lecture
if (lfile .ne. mfile) goto 10
c
c ********************************************************
c*FON Recherche du bloc contenant la date passee en parametre
c ********************************************************
c
c -----------------------------------------------------------------
c*FON Gestion de la lecture en fonction de la date du dernier bloc lu.
c*FON Lecture des blocs anterieurs si la date est < date debut du bloc.
c*FON Lecture des blocs suivants si la date est > date fin du bloc.
c*FON Pas de lecture dans les autres cas.
c ------------------------------------------------------------------
c
if (day .gt. dayend + 1.d-4) goto 20
if (day .ge. daybeg - 1.d-4) goto 70
c
c ------------------------------------------------------------------
c*FON Initialisation de la lecture et positionnement en debut de fichier
c ------------------------------------------------------------------
c
10 continue
dayfir = 99.d9
daylas = 99.d9
mfile = lfile
rewind lfile
c
c -------------------------------------------------------------
c*FON Recherche du 1er enregistrement du bloc (numero du satellite)
c -------------------------------------------------------------
c
20 continue
read (lfile,1000,err=901,end=100) nsat
if (nsat .le. 0) goto 20
if (nsat .gt. 4) goto 20
c
c* -----------------------------
c*FON Lecture du 2nd enregistrenent
c* -----------------------------
c*FON structure de l'enregistrement :
c*FON nrec = identification de l'enreg (doit etre egal a 200 + nsat)
c*FON daybeg = date debut de l'enreg (date julienne)
c*FON dayend = date fin de l'enreg (date julienne)
c*FON epoch = epoque de reference du vecteur etat (date julienne)
c*FON revepo = numero d'orbite
c*FON smaxis = demi grand axe en km
c*FON omotin = mouvement moyen inverse pour une orbite Keplerienne
c
read (lfile,1100,err=901,end=100)
> nrec,daybeg,dayend,epoch,revepo,smaxis,omotin
c
c ----------------------------------
c*FON Verification du 2nd enregistrement
c ----------------------------------
c
if (nrec .ne. 200 + nsat) goto 900
if (daybeg .gt. dayend) goto 900
c
c ------------------------------------------------------------------
c*FON Controle de la date et de la continuite des enregistrements
c*FON La date doit etre > la + petite date deja lu - marge
c*FON L'ecart entre la date fin de l'enreg precedent et la date debut de
c*FON l'enreg courant doit etre < gap
c*FON marge = 1.d-4 et gap = 2.d-4
c ------------------------------------------------------------------
c
dayfir = dmin1(dayfir,daybeg - 1.d-4)
if (day .lt. dayfir) goto 905
if (daybeg .gt. daylas) goto 903
c
daylas = dayend + 2.d-4
c
c ------------------------------------------------------------------
c*FON Gestion de la lecture en fonction de la date du bloc :
c*FON date < date debut du bloc - marge : lecture des blocs anterieurs
c*FON date > date fin du bloc + marge : lecture des blocs suivants
c ------------------------------------------------------------------
c
if (day .gt. dayend + 1.d-4) goto 20
if (day .lt. daybeg - 1.d-4) goto 10
c
c -----------------------------
c*FON Lecture du 3ieme enregistrenent
c* -----------------------------
c*FON structure de l'enregistrement :
c*FON nrec = id de l`enreg (doit etre egal a 300 + nb de coeff polynomiaux)
c*FON y(6) = vecteur d'etat pour l'orbite de Kepler (km, km/s)
c*FON rdist = distance centre terre satellite a la date courante en km
c
read (lfile,1200,err=901,end=900) nrec,y,rdist
c
c ------------------------------------
c*FON verification du 3ieme enregistrement
c ------------------------------------
c
if (nrec .gt. 310) goto 900
if (nrec .lt. 300) goto 900
koeff = nrec - 300
c
if (koeff. le. 0) goto 70
c
c ----------------------------------------------------------
c*FON Lecture des coefficients du polynome de Chebyshev pour les
c*FON 6 composants du vecteur d'etat
c* ----------------------------------------------------------
c
do 60 k = 1, koeff
read (lfile,1300,err=901,end=900) nrec,(coeff(k,i),i=1,6)
c
c ------------------------------------------------------
c*FON Verification de l'enregistrement
c*FON nrec = id de l`enreg (doit etre egal a koeff + 11 * k)
c* ------------------------------------------------------
c
if (11 * k + koeff .ne. nrec) goto 900
60 continue
c
c* -----------------
c*FON Fin de la lecture
c* -----------------
c
70 continue
c
c *****************************************
c*FON Debut de resolution de l'orbite de Kepler
c *****************************************
c
c -------------------------------------------------------
c*FON Convertion de la difference de date en anomalie moyenne
c*FON Calcul du numero d'orbite
c -------------------------------------------------------
c
dmanom = (day - epoch) * 864.d2 / omotin
revnum = revepo + dmanom / 6.2831853072d0
c
arin = smaxis / rdist
arm = (rdist - smaxis) / smaxis
rvwam = (y(1) * y(4) + y(2) * y(5) + y(3) * y(6)) * omotin
> / smaxis**2
c
c* --------------------------------------------------------
c*FON Calcul de l'anomalie excentrique par iteration de Newton
c* --------------------------------------------------------
c
tam = dmanom - rvwam
comp = 1.d-7 + 1.d-10 * dabs(tam)
b = tam
c
c ---------------------------------------------
c*FON Iterations pour resoudre l'equation de Kepler
c ---------------------------------------------
c
do 130 iter = 1, 15
go = cos(b)
g1 = sin(b)
bet = tam - arm * g1 + rvwam * go
d = (bet - b) / (1.d0 + arm * go + rvwam * g1)
b = b + d
c
c -----------------
c*FON Condition d'arret
c -----------------
c
if (dabs(d) .le. comp) goto 140
130 continue
c
c ------------------
c*FON Pas de convergence
c ------------------
c
goto 900
140 continue
go = go - d * g1
g1 = g1 + d * go
g2 = 1.d0 - go
g3 = b - g1
fx = 1.d0 - g2 * arin
gx = (dmanom - g3) * omotin
c
k = min0(kode,3)
do 150 j = 1, k
x(j) = fx * y(j) + gx * y(j+3)
150 continue
c
if (kode .le. 3) goto 170
rx = sqrt(x(1)**2 + x(2)**2 + x(3)**2)
ft = -g1 * smaxis * arin / (omotin * rx)
gt = 1.d0 - g2 * smaxis / rx
do 160 j = 4, kode
x(j) = ft * y(j-3) + gt * y(j)
160 continue
c
c* ******************************************
c*FON Fin de la resolution de l'orbite de Kepler
c* ******************************************
c
170 continue
c
c ---------------------------------------------------------
c*FON Verification des coeff polynomiaux (1 n'est pas autorise)
c ---------------------------------------------------------
c
if (koeff .le. 1) goto 600
c
c ----------------------------------------------------------------------
c*FON Date milieu et vecteur d'echelle pour le coeff polynomial de Chebyshev
c ----------------------------------------------------------------------
c
daymid = 0.5d0 * (daybeg + dayend)
scale = 4.d0 / (dayend - daybeg)
s = scale * (day - daymid)
pa = 1.d0
p = s * 0.5d0
c
c --------------------------------
c*FON Initialisation du vecteur d'etat
c --------------------------------
c
do 200 j = 1, kode
x(j) = x(j) + coeff(1,j) + coeff(2,j) * p
200 continue
c
if (koeff .le. 2) goto 600
do 210 l = 3, koeff
pb = pa
pa = p
p = s * pa - pb
do 220 j = 1, kode
x(j) = x(j) + coeff(l,j)*p
220 continue
210 continue
c
goto 600
c
c -------------------
c*FON Gestion des erreurs
c -------------------
c
100 continue
c
c ---------------------------------------------------
c*FON Cas de la fin de fichier :
c*FON Cas de date trop grande si le 1er enreg est deja lu
c*FON Donnees invalide dans les autres cas
c ---------------------------------------------------
c
if (daylas .lt. 1.d9) goto 904
c
900 continue
ier = -1
901 continue
ier = ier + 2
902 continue
ier = ier + 1
903 continue
ier = ier + 1
904 continue
ier = ier + 1
905 continue
ier = ier + 1
c
c -----------------------------------------------------------
c*FON Re_initialisation de mfile pour forcer une lecture conplete
c*FON au prochain appel
c -----------------------------------------------------------
c
mfile = -9999
600 continue
c
c ****************
c Fin de programme
c ****************
c
return
1000 format(i3)
1100 format(i3,2f12.6,f15.9,f11.3,2f13.5)
1200 format(i3,3f11.3,3f11.7,f11.3)
1300 format(i3,3f11.3,3f11.7)
end