(defun hfti (a mda m n b mdb nb tau krank rnorm h g ip) (declare (type (simple-array double-float (* *)) a)) (declare (type fixnum mda)) (declare (type fixnum m)) (declare (type fixnum n)) (declare (type (simple-array double-float (* *)) b)) (declare (type fixnum mdb)) (declare (type fixnum nb)) (declare (type double-float tau)) (declare (type fixnum krank)) (declare (type (simple-array double-float (*)) rnorm)) (declare (type (simple-array double-float (*)) h)) (declare (type (simple-array double-float (*)) g)) (declare (type (simple-array fixnum (*)) ip)) (prog ((sm 0.0d0) (dzero 0.0d0) (jj 0) (sm1 0.0d0) (ip1 0) (ii 0) (jb 0) (kp1 0) (tmp 0.0d0) (i 0) (hmax 0.0d0) (l 0) (lmax 0) (j 0) (ldiag 0) (k 0) (factor 0.0d0) (szero 0.0d0) ) (declare (type double-float sm)) (declare (type double-float dzero)) (declare (type fixnum jj)) (declare (type double-float sm1)) (declare (type fixnum ip1)) (declare (type fixnum ii)) (declare (type fixnum jb)) (declare (type fixnum kp1)) (declare (type double-float tmp)) (declare (type fixnum i)) (declare (type double-float hmax)) (declare (type fixnum l)) (declare (type fixnum lmax)) (declare (type fixnum j)) (declare (type fixnum ldiag)) (declare (type fixnum k)) (declare (type double-float factor)) (declare (type double-float szero)) (comment " c.l.lawson and r.j.hanson, jet propulsion laboratory, 1973 jun 12" ) (comment " solve least squares problem using algorithm, hfti.") (comment " to appear in @solving least squares problems@, prentice-hall, 1974" ) (comment "") (setf szero 0.0) (setf dzero 0.0d0) (setf factor 0.001) (comment "") (setf k 0) (setf ldiag (min0 m n)) (if (<= ldiag 0) (go label270)) (fdo ((j 1 (+ j 1))) ((> j ldiag) nil) (tagbody (if (= j 1) (go label20)) (comment "") (comment " update squared column lengths and find lmax") (comment " ..") (setf lmax j) (fdo ((l j (+ l 1))) ((> l n) nil) (tagbody (fset (fref h l) (+ (fref h l) (- (expt (fref a (+ j (- 1)) l) 2)))) (if (> (fref h l) (fref h lmax)) (setf lmax l)) )) (arithmetic-if (diff (+ hmax (* factor (fref h lmax))) hmax) (go label20) (go label20) (go label50) ) (comment "") (comment " compute squared column lengths and find lmax") (comment " ..") label20 (setf lmax j) (fdo ((l j (+ l 1))) ((> l n) nil) (tagbody (fset (fref h l) 0.0) (fdo ((i j (+ i 1))) ((> i m) nil) (tagbody (fset (fref h l) (+ (fref h l) (expt (fref a i l) 2)))) ) (if (> (fref h l) (fref h lmax)) (setf lmax l)) )) (setf hmax (fref h lmax)) (comment " ..") (comment " lmax has been determined") (comment "") (comment " do column interchanges if needed.") (comment " ..") label50 (fset (fref ip j) lmax) (if (= (fref ip j) j) (go label70)) (fdo ((i 1 (+ i 1))) ((> i m) nil) (tagbody (setf tmp (fref a i j)) (fset (fref a i j) (fref a i lmax)) (fset (fref a i lmax) tmp) )) (fset (fref h lmax) (fref h j)) (comment "") (comment " compute the j-th transformation and apply it to a and b.") (comment " ..") label70 (multiple-value-setq (dummy_var j dummy_var m dummy_var dummy_var dummy_var dummy_var dummy_var mda dummy_var ) (h12 1 j (+ j 1) m (fref a 1 j) 1 (fref h j) (fref a 1 (+ j 1)) 1 mda (+ n (- j)) )) (multiple-value-setq (dummy_var j dummy_var m dummy_var dummy_var dummy_var b dummy_var mdb nb) (h12 2 j (+ j 1) m (fref a 1 j) 1 (fref h j) b 1 mdb nb) ))) (comment "") (comment " determine the pseudorank, k, using the tolerance, tau.") (comment " ..") (fdo ((j 1 (+ j 1))) ((> j ldiag) nil) (tagbody (if (<= (abs (fref a j j)) tau) (go label100))) ) (setf k ldiag) (go label110) label100 (setf k (+ j (- 1))) label110 (setf kp1 (+ k 1)) (comment "") (comment " compute the norms of the residual vectors.") (comment "") (if (<= nb 0) (go label140)) (fdo ((jb 1 (+ jb 1))) ((> jb nb) nil) (tagbody (setf tmp szero) (if (> kp1 m) (go label130)) (fdo ((i kp1 (+ i 1))) ((> i m) nil) (tagbody (setf tmp (+ tmp (expt (fref b i jb) 2)))) ) label130 (fset (fref rnorm jb) (sqrt tmp)) )) label140 (comment " special for pseudorank = 0" ) (if (> k 0) (go label160)) (if (<= nb 0) (go label270)) (fdo ((jb 1 (+ jb 1))) ((> jb nb) nil) (tagbody (fdo ((i 1 (+ i 1))) ((> i n) nil) (tagbody (fset (fref b i jb) szero))) )) (go label270) (comment "") (comment " if the pseudorank is less than n compute householder") (comment " decomposition of first k rows.") (comment " ..") label160 (if (= k n) (go label180)) (fdo ((ii 1 (+ ii 1))) ((> ii k) nil) (tagbody (setf i (+ kp1 (- ii))) (multiple-value-setq (dummy_var i kp1 n dummy_var mda dummy_var a mda dummy_var dummy_var) (h12 1 i kp1 n (fref a i 1) mda (fref g i) a mda 1 (+ i (- 1))) ))) label180 (comment "") (comment "") (if (<= nb 0) (go label270)) (fdo ((jb 1 (+ jb 1))) ((> jb nb) nil) (tagbody (comment "") (comment " solve the k by k triangular system.") (comment " ..") (fdo ((l 1 (+ l 1))) ((> l k) nil) (tagbody (setf sm dzero) (setf i (+ kp1 (- l))) (if (= i k) (go label200)) (setf ip1 (+ i 1)) (fdo ((j ip1 (+ j 1))) ((> j k) nil) (tagbody (setf sm (+ sm (* (fref a i j) (dble (fref b j jb)))))) ) label200 (setf sm1 sm) (fset (fref b i jb) (/ (+ (fref b i jb) (- sm1)) (fref a i i))) )) (comment "") (comment " complete computation of solution vector.") (comment " ..") (if (= k n) (go label240)) (fdo ((j kp1 (+ j 1))) ((> j n) nil) (tagbody (fset (fref b j jb) szero))) (fdo ((i 1 (+ i 1))) ((> i k) nil) (tagbody (multiple-value-setq (dummy_var i kp1 n dummy_var mda dummy_var dummy_var dummy_var mdb dummy_var ) (h12 2 i kp1 n (fref a i 1) mda (fref g i) (fref b 1 jb) 1 mdb 1) ))) (comment "") (comment " re-order the solution vector to compensate for the") (comment " column interchanges.") (comment " ..") label240 (fdo ((jj 1 (+ jj 1))) ((> jj ldiag) nil) (tagbody (setf j (+ (+ ldiag 1) (- jj))) (if (= (fref ip j) j) (go label250)) (setf l (fref ip j)) (setf tmp (fref b l jb)) (fset (fref b l jb) (fref b j jb)) (fset (fref b j jb) tmp) label250 )))) (comment " ..") (comment " the solution vectors, x, are now") (comment " in the first n rows of the array b(,).") (comment "") label270 (setf krank k) (return (values a mda m n b mdb nb tau krank rnorm h g ip)) ))