| 1 | SUBROUTINE FELLIPSE(NMAX,X,Y,BB,TAU,RNORM) |
|---|
| 2 | |
|---|
| 3 | Cf2py intent(in) NMAX |
|---|
| 4 | Cf2py intent(in) X,Y |
|---|
| 5 | Cf2py intent(in) Y |
|---|
| 6 | Cf2py depend(NMAX) X,Y |
|---|
| 7 | Cf2py intent(in) TAU |
|---|
| 8 | Cf2py intent(out) BB |
|---|
| 9 | Cf2py intent(out) RNORM |
|---|
| 10 | |
|---|
| 11 | C |
|---|
| 12 | C NMAX is the number of data points |
|---|
| 13 | C A, B : work arrays |
|---|
| 14 | C X, Y : data points coordinates |
|---|
| 15 | C |
|---|
| 16 | C Daniel Pfenniger, Geneva Observatory, 6/1991 |
|---|
| 17 | C |
|---|
| 18 | C |
|---|
| 19 | REAL*8 X(NMAX),Y(NMAX) |
|---|
| 20 | REAL*4 A(5,NMAX),B(NMAX),BB(0:4),TAU |
|---|
| 21 | INTEGER*4 KRANK,NMAX |
|---|
| 22 | REAL*4 RNORM |
|---|
| 23 | C |
|---|
| 24 | C |
|---|
| 25 | C |
|---|
| 26 | CALL FITQDR(NMAX,NMAX,A,B,X,Y,TAU,KRANK,RNORM) |
|---|
| 27 | |
|---|
| 28 | C Coefficients of the quadratic form |
|---|
| 29 | C |
|---|
| 30 | BB(0) = B(1) |
|---|
| 31 | BB(1) = B(2) |
|---|
| 32 | BB(2) = B(3) |
|---|
| 33 | BB(3) = B(4) |
|---|
| 34 | BB(4) = B(5) |
|---|
| 35 | C |
|---|
| 36 | RETURN |
|---|
| 37 | END |
|---|
| 38 | C----------------------------------------------------------------------- |
|---|
| 39 | C |
|---|
| 40 | SUBROUTINE FITQDR(N,NMAX,A,B,X,Y,TAU,KRANK,RNORM) |
|---|
| 41 | C |
|---|
| 42 | C Fortran subroutine using the linear least squares routine HFTI |
|---|
| 43 | C finding the parameters a, c, d, e, f defining the best |
|---|
| 44 | C quadratic form |
|---|
| 45 | C |
|---|
| 46 | C (1+a) X^2 + (1-a) Y^2 + c X Y + d X + e Y + f = 0 |
|---|
| 47 | C |
|---|
| 48 | C passing through a set of data points {Xi,Yi} i=1,..N |
|---|
| 49 | C The result is invariant by rotation and translation. |
|---|
| 50 | C If N<6, this program finds an exact solution passing through the |
|---|
| 51 | C points |
|---|
| 52 | C If N>5, it finds the quadratic form which minimizes |
|---|
| 53 | C |
|---|
| 54 | C SUM(i=1,N) [ (1+a)X^2 + (1-a)Y^2 + c X Y + d X + e Y + f ]^2 |
|---|
| 55 | C |
|---|
| 56 | C N : Actual number of data points |
|---|
| 57 | C NMAX : Maximum number of data points |
|---|
| 58 | C NPAR : Number of parameters to find (5) |
|---|
| 59 | C A, B : Work arrays |
|---|
| 60 | C X, Y : Data point coordinates |
|---|
| 61 | C KRANK : Rank of the data matrix A (should be 5) |
|---|
| 62 | C RNORM : Norm of the residual |
|---|
| 63 | C |
|---|
| 64 | C TAU : tolerance for a zero in HFTI |
|---|
| 65 | C |
|---|
| 66 | C On outpout a,c,d,e,f are in B(1),B(2), ... B(5) respectively |
|---|
| 67 | C |
|---|
| 68 | C Daniel Pfenniger, Geneva Observatory, 6/1991 |
|---|
| 69 | C |
|---|
| 70 | PARAMETER (NPAR=5) |
|---|
| 71 | C |
|---|
| 72 | REAL*4 A(NMAX,NPAR), B(NMAX) |
|---|
| 73 | REAL*8 X(NMAX), Y(NMAX) |
|---|
| 74 | C |
|---|
| 75 | C H, G, IP : Work arrays for HFTI |
|---|
| 76 | C |
|---|
| 77 | INTEGER*4 H(NPAR), IP(NPAR),I,N |
|---|
| 78 | REAL*4 G(NPAR) |
|---|
| 79 | C |
|---|
| 80 | C Building the matrices A and B |
|---|
| 81 | C |
|---|
| 82 | DO 10 I = 1, N |
|---|
| 83 | A(I,1) = X(I)**2-Y(I)**2 |
|---|
| 84 | A(I,2) = X(I)*Y(I) |
|---|
| 85 | A(I,3) = X(I) |
|---|
| 86 | A(I,4) = Y(I) |
|---|
| 87 | A(I,5) = 1. |
|---|
| 88 | 10 B(I) = -(X(I)**2+Y(I)**2) |
|---|
| 89 | C |
|---|
| 90 | C Solving the LS problem: ||A Z - B|| minimum |
|---|
| 91 | C Call HFTI, the result Z is in the first NPAR rows of B |
|---|
| 92 | C TAU is the tolerance for zero |
|---|
| 93 | C KRANK is the rank of A (should be NPAR) |
|---|
| 94 | C RNORM is the norm of the residual |
|---|
| 95 | C See Lawson & Hanson (1974) for detail |
|---|
| 96 | CALL HFTI(A,NMAX,N,NPAR,B,NMAX,1,TAU,KRANK,RNORM,H,G,IP) |
|---|
| 97 | C |
|---|
| 98 | RETURN |
|---|
| 99 | END |
|---|
| 100 | |
|---|
| 101 | C Some LSQ routines from Lawson & Hanson |
|---|
| 102 | C |
|---|
| 103 | SUBROUTINE HFTI (A,MDA,M,N,B,MDB,NB,TAU,KRANK,RNORM,H,G,IP) |
|---|
| 104 | C C.L.LAWSON & R.J.HANSON, JET PROPULSION LABORATORY, 1973 JUN 12 |
|---|
| 105 | C TO APPEAR IN 'SOLVING LEAST SQUARES PROBLEMS', PRENTICE-HALL 1974 |
|---|
| 106 | C SOLVE LEAST SQUARES PROBLEM USING ALGORITHM HFTI. |
|---|
| 107 | C |
|---|
| 108 | DIMENSION A(MDA,N),B(MDB,NB),H(N),G(N),RNORM(NB) |
|---|
| 109 | INTEGER IP(N) |
|---|
| 110 | DOUBLE PRECISION SM,DZERO |
|---|
| 111 | PARAMETER (SZERO=0., DZERO=0.D0, FACTOR = 0.001 ) |
|---|
| 112 | C |
|---|
| 113 | K = 0 |
|---|
| 114 | LDIAG = MIN0(M,N) |
|---|
| 115 | IF (LDIAG.LE.0) GOTO 270 |
|---|
| 116 | DO 80 J=1,LDIAG |
|---|
| 117 | IF (J.EQ.1) GOTO 20 |
|---|
| 118 | C |
|---|
| 119 | C UPDATE SQUARED COLUMN LENGTHS AND FIND LMAX |
|---|
| 120 | C |
|---|
| 121 | LMAX = J |
|---|
| 122 | DO 10 L = J,N |
|---|
| 123 | H(L) = H(L) - A(J-1,L)**2 |
|---|
| 124 | IF (H(L).GT.H(LMAX)) LMAX = L |
|---|
| 125 | 10 CONTINUE |
|---|
| 126 | IF (DIFF(HMAX+FACTOR*H(LMAX),HMAX)) 20,20,50 |
|---|
| 127 | C |
|---|
| 128 | C COMPUTE SQUARED COLUMN LENGTHS AND FIND LMAX |
|---|
| 129 | C |
|---|
| 130 | 20 LMAX = J |
|---|
| 131 | DO 40 L = J,N |
|---|
| 132 | H(L) = SZERO |
|---|
| 133 | DO 30 I = J,M |
|---|
| 134 | 30 H(L) = H(L) + A(I,L)**2 |
|---|
| 135 | IF (H(L).GT.H(LMAX)) LMAX = L |
|---|
| 136 | 40 CONTINUE |
|---|
| 137 | HMAX = H(LMAX) |
|---|
| 138 | C |
|---|
| 139 | C LMAX HAS BEEN DETERMINED |
|---|
| 140 | C |
|---|
| 141 | C DO COLUMN INTERCHANGES IF NEEDED. |
|---|
| 142 | C |
|---|
| 143 | 50 CONTINUE |
|---|
| 144 | IP(J) = LMAX |
|---|
| 145 | IF (IP(J).EQ.J) GOTO 70 |
|---|
| 146 | DO 60 I = 1,M |
|---|
| 147 | TMP = A(I,J) |
|---|
| 148 | A(I,J) = A(I,LMAX) |
|---|
| 149 | 60 A(I,LMAX) = TMP |
|---|
| 150 | H(LMAX) = H(J) |
|---|
| 151 | C |
|---|
| 152 | C COMPUTE THE J-TH TRANSFORMATION AND APPLY IT TO A AND B. |
|---|
| 153 | C |
|---|
| 154 | 70 CALL H12 (1,J,J+1,M,A(1,J),1,H(J),A(1,J+1),1,MDA,N-J) |
|---|
| 155 | 80 CALL H12 (2,J,J+1,M,A(1,J),1,H(J),B,1,MDB,NB) |
|---|
| 156 | C |
|---|
| 157 | C DETERMINE THE PSEUDORANK, K, USING THE TOLERANCE, TAU. |
|---|
| 158 | C |
|---|
| 159 | DO 90 J = 1,LDIAG |
|---|
| 160 | IF (ABS(A(J,J)).LE.TAU) GOTO 100 |
|---|
| 161 | 90 CONTINUE |
|---|
| 162 | K = LDIAG |
|---|
| 163 | GOTO 110 |
|---|
| 164 | 100 K = J - 1 |
|---|
| 165 | 110 KP1 = K + 1 |
|---|
| 166 | C |
|---|
| 167 | C COMPUTE THE NORMS OF THE RESIDUAL VECTORS. |
|---|
| 168 | C |
|---|
| 169 | IF (NB.LE.0) GOTO 140 |
|---|
| 170 | DO 130 JB = 1,NB |
|---|
| 171 | TMP = SZERO |
|---|
| 172 | IF (KP1.GT.M) GOTO 130 |
|---|
| 173 | DO 120 I = KP1,M |
|---|
| 174 | 120 TMP = TMP + B(I,JB)**2 |
|---|
| 175 | 130 RNORM(JB) = SQRT(TMP) |
|---|
| 176 | 140 CONTINUE |
|---|
| 177 | C SPECIAL FOR PSEUDORANK = 0 |
|---|
| 178 | IF (K.GT.0) GOTO 160 |
|---|
| 179 | IF (NB.LE.0) GOTO 270 |
|---|
| 180 | DO 150 JB = 1,NB |
|---|
| 181 | DO 150 I = 1,N |
|---|
| 182 | 150 B(I,JB) = SZERO |
|---|
| 183 | GOTO 270 |
|---|
| 184 | C |
|---|
| 185 | C IF THE PSEUDORANK IS LESS THAN N COMPUTE HOUSHOLDER |
|---|
| 186 | C DECOMPOSITION OF FIRST K ROWS. |
|---|
| 187 | C |
|---|
| 188 | 160 IF (K.EQ.N) GOTO 180 |
|---|
| 189 | DO 170 II = 1,K |
|---|
| 190 | I = KP1 - II |
|---|
| 191 | 170 CALL H12 (1,I,KP1,N,A(I,1),MDA,G(I),A,MDA,1,I-1) |
|---|
| 192 | 180 CONTINUE |
|---|
| 193 | C |
|---|
| 194 | C |
|---|
| 195 | IF (NB.LE.0) GOTO 270 |
|---|
| 196 | DO 260 JB = 1,NB |
|---|
| 197 | C |
|---|
| 198 | C SOLVE THE K BY K TRIANGULAR SYSTEM |
|---|
| 199 | C |
|---|
| 200 | DO 210 L = 1,K |
|---|
| 201 | SM = DZERO |
|---|
| 202 | I = KP1 - L |
|---|
| 203 | IF (I.EQ.K) GOTO 200 |
|---|
| 204 | IP1 = I + 1 |
|---|
| 205 | DO 190 J = IP1,K |
|---|
| 206 | 190 SM = SM + A(I,J)*DBLE(B(J,JB)) |
|---|
| 207 | 200 SM1 = SM |
|---|
| 208 | 210 B(I,JB) = (B(I,JB)-SM1)/A(I,I) |
|---|
| 209 | C |
|---|
| 210 | IF (K.EQ.N) GOTO 240 |
|---|
| 211 | DO 220 J = KP1,N |
|---|
| 212 | 220 B(J,JB) = SZERO |
|---|
| 213 | DO 230 I = 1,K |
|---|
| 214 | 230 CALL H12 (2,I,KP1,N,A(I,1),MDA,G(I),B(1,JB),1,MDB,1) |
|---|
| 215 | C |
|---|
| 216 | C RE-ORDER THE SOLUTION VECTOR TO COMPENSATE FOR THE |
|---|
| 217 | C COLUMN INTERCHANGES. |
|---|
| 218 | C |
|---|
| 219 | C COMPLETE COMPUTATION OF SOLUTION VECTOR |
|---|
| 220 | C |
|---|
| 221 | 240 DO 250 JJ = 1,LDIAG |
|---|
| 222 | J = LDIAG + 1 - JJ |
|---|
| 223 | IF (IP(J).EQ.J) GOTO 250 |
|---|
| 224 | L = IP(J) |
|---|
| 225 | TMP = B(L,JB) |
|---|
| 226 | B(L,JB) = B(J,JB) |
|---|
| 227 | B(J,JB) = TMP |
|---|
| 228 | 250 CONTINUE |
|---|
| 229 | 260 CONTINUE |
|---|
| 230 | C |
|---|
| 231 | C THE SOLUTION VECTORS, X, ARE NOW |
|---|
| 232 | C IN THE FIRST N ROWS OF THE ARRAY B(.). |
|---|
| 233 | C |
|---|
| 234 | 270 KRANK = K |
|---|
| 235 | RETURN |
|---|
| 236 | END |
|---|
| 237 | C----------------------------------------------------------------------- |
|---|
| 238 | C |
|---|
| 239 | C SUBROUTINE H12 (MODE,LPIVOT,L1,M,U,IUE,UP,C,ICE,ICV,NCV) |
|---|
| 240 | C C.L.LAWSON & R.J.HANSON, JET PROPULSION LABORATORY, 1973 JUN 12 |
|---|
| 241 | C TO APPEAR IN 'SOLVING LEAST SQUARES PROBLEMS', PRENTICE-HALL, 1974 |
|---|
| 242 | C |
|---|
| 243 | C CONSTRUCTION AND/OR APPLICATION OF A SINGLE |
|---|
| 244 | C HOUSHOLDER TRANSFORMATION: Q = I + U*(U**T)/B |
|---|
| 245 | C |
|---|
| 246 | C MODE = 1 OR 2 TO SELECT ALGORITHM H1 OR H2 |
|---|
| 247 | C LPIVOT IS THE INDEX OF THE PIVOT ELEMENT. |
|---|
| 248 | C L1,M IF L1 .LE. M THE TRANSFORMATION WILL BE CONSTRUCTED TO |
|---|
| 249 | C ZERO ELEMENTS INDEXED FROM L1 THROUGH M. IF L1 .GT. M |
|---|
| 250 | C THE SUBROUTINE DOES AN IDENTITY TRANSFORMATION. |
|---|
| 251 | C U(),IUE,UP ON ENTRY TO H1 U() CONTAINS THE PIVOT VECTOR. |
|---|
| 252 | C IUE IS THE STORAGE INCREMENT BETWEEN ELEMENTS. |
|---|
| 253 | C ON EXIT FROM H1 U() AND UP |
|---|
| 254 | C CONTAIN QUANTITES DEFINING THE VECTOR U OF THE |
|---|
| 255 | C HOUSHOLDER TRANSFORMATION. ON ENTRY TO H2 U() |
|---|
| 256 | C AND UP SHOULD CONTAIN QUATITIES PREVIOUSLY COMPUTED |
|---|
| 257 | C BY H1. THESE WILL NOT BE MODIFIED BY H2. |
|---|
| 258 | C C() ON ENTRY TO H1 OR H2 C() CONTAINS A MATRIX WHICH WILL BE |
|---|
| 259 | C REGARDED AS A SET OF VECTORS TO WHICH THE HOUSHOLDER |
|---|
| 260 | C TRANSFORMATION IS TO BE APPLIED. ON EXIT C() CONTAINS THE |
|---|
| 261 | C SET OF TRANSFORMED VECTORS. |
|---|
| 262 | C ICE STORAGE INCREMENT BETWEEN ELEMENTS OF VECTORS IN C(). |
|---|
| 263 | C ICV STORAGE INCREMENT BETWEEN VECTORS IN C(). |
|---|
| 264 | C NCV NUMBER OF VECTORS IN C() TO BE TRANSFORMED. IF NCV .LE. 0 |
|---|
| 265 | C NO OPERATIONS WILL BE DONE ON C(). |
|---|
| 266 | C |
|---|
| 267 | SUBROUTINE H12 (MODE,LPIVOT,L1,M,U,IUE,UP,C,ICE,ICV,NCV) |
|---|
| 268 | DIMENSION U(IUE,M),C(*) |
|---|
| 269 | DOUBLE PRECISION SM,B |
|---|
| 270 | PARAMETER (ONE = 1.) |
|---|
| 271 | C |
|---|
| 272 | IF (0.GE.LPIVOT.OR.LPIVOT.GE.L1.OR.L1.GT.M) RETURN |
|---|
| 273 | CL = ABS(U(1,LPIVOT)) |
|---|
| 274 | IF (MODE.EQ.2) GOTO 60 |
|---|
| 275 | C *** CONSTRUCT THE TRANSFORMATION *** |
|---|
| 276 | DO 10 J = L1,M |
|---|
| 277 | 10 CL = AMAX1(ABS(U(1,J)),CL) |
|---|
| 278 | IF (CL) 130,130,20 |
|---|
| 279 | 20 CLINV = ONE/CL |
|---|
| 280 | SM = (DBLE(U(1,LPIVOT))*CLINV)**2 |
|---|
| 281 | DO 30 J = L1,M |
|---|
| 282 | 30 SM = SM + (DBLE(U(1,J))*CLINV)**2 |
|---|
| 283 | C CONVERT DBLE PREC SM TO SNGL. PREC. SM1 |
|---|
| 284 | SM1 = SM |
|---|
| 285 | CL = CL*SQRT(SM1) |
|---|
| 286 | IF (U(1,LPIVOT)) 50,50,40 |
|---|
| 287 | 40 CL = -CL |
|---|
| 288 | 50 UP = U(1,LPIVOT) - CL |
|---|
| 289 | U(1,LPIVOT) = CL |
|---|
| 290 | GOTO 70 |
|---|
| 291 | C *** APPLY THE TRANSFORMATION I + U*(U**T)*B TO C *** |
|---|
| 292 | C |
|---|
| 293 | 60 IF (CL) 130,130,70 |
|---|
| 294 | 70 IF (NCV.LE.0) RETURN |
|---|
| 295 | B = DBLE(UP)*U(1,LPIVOT) |
|---|
| 296 | C B MUST BE NONPOSITIVE HERE. IF B=0.,RETURN |
|---|
| 297 | C |
|---|
| 298 | IF (B) 80,130,130 |
|---|
| 299 | 80 B = ONE/B |
|---|
| 300 | I2 = 1 - ICV + ICE*(LPIVOT-1) |
|---|
| 301 | INCR = ICE*(L1-LPIVOT) |
|---|
| 302 | DO 120 J = 1,NCV |
|---|
| 303 | I2 = I2 +ICV |
|---|
| 304 | I3 = I2 + INCR |
|---|
| 305 | I4 = I3 |
|---|
| 306 | SM = C(I2)*DBLE(UP) |
|---|
| 307 | DO 90 I = L1,M |
|---|
| 308 | SM = SM + C(I3)*DBLE(U(1,I)) |
|---|
| 309 | 90 I3 = I3 + ICE |
|---|
| 310 | IF (SM) 100,120,100 |
|---|
| 311 | 100 SM = SM*B |
|---|
| 312 | C(I2) = C(I2) + SM*DBLE(UP) |
|---|
| 313 | DO 110 I = L1,M |
|---|
| 314 | C(I4) = C(I4) + SM*DBLE(U(1,I)) |
|---|
| 315 | 110 I4 = I4 + ICE |
|---|
| 316 | 120 CONTINUE |
|---|
| 317 | 130 RETURN |
|---|
| 318 | END |
|---|
| 319 | C--------------------------- this routine is not a joke! |
|---|
| 320 | C |
|---|
| 321 | FUNCTION DIFF(X,Y) |
|---|
| 322 | C C.L.LAWSON & R.J.HANSON, JET PROPULSION LABORATORY, 1973 JUNE 7 |
|---|
| 323 | C TO APPEAR IN 'SOLVING LEAST SQUARES PROBLEMS', PRENTICE-HALL, 1974 |
|---|
| 324 | DIFF = X-Y |
|---|
| 325 | RETURN |
|---|
| 326 | END |
|---|
