34 lines
1.3 KiB
Common Lisp
34 lines
1.3 KiB
Common Lisp
;; Solve a linear system AX=B where A is symmetric and positive definite, so it can be Cholesky decomposed.
|
|
(defun linsys (A B)
|
|
(let* ((n (car (array-dimensions A)))
|
|
(m (cadr (array-dimensions B)))
|
|
(y (make-array n :element-type 'long-float :initial-element 0.0L0))
|
|
(X (make-array `(,n ,m) :element-type 'long-float :initial-element 0.0L0))
|
|
(L (chol A))) ; A=LL'
|
|
|
|
(loop for col from 0 to (- m 1) do
|
|
;; Forward substitution: y = L\B
|
|
(loop for k from 0 to (- n 1)
|
|
do (setf (aref y k)
|
|
(/ (- (aref B k col)
|
|
(loop for j from 0 to (- k 1)
|
|
sum (* (aref L k j)
|
|
(aref y j))))
|
|
(aref L k k))))
|
|
|
|
;; Back substitution. x=L'\y
|
|
(loop for k from (- n 1) downto 0
|
|
do (setf (aref X k col)
|
|
(/ (- (aref y k)
|
|
(loop for j from (+ k 1) to (- n 1)
|
|
sum (* (aref L j k)
|
|
(aref X j col))))
|
|
(aref L k k)))))
|
|
X))
|
|
|
|
;; Solve a linear least squares problem. Ax=b, with A being mxn, with m>n.
|
|
;; Solves the linear system A'Ax=A'b.
|
|
(defun lsqr (A b)
|
|
(linsys (mmul (mtp A) A)
|
|
(mmul (mtp A) b)))
|