Topic: Least squares solution

[chlh_jd]

Hi, All .
I have overdetermined equations ,  and want to use the method of least squares solution .
 GPS positioning model : ρj=Sqrt ((X-Xj)^2+(Y-Yj)^2+(Z-Zj)^2)
  Now I want the point(X Y Z) where I am,

Code: [Select]

;;Satellite coordinates (Xj Yj Zj)
'((16414028.668 660383.618 20932036.907)
  (16896800.648 -18784061.365 -7418318.856)
  (9339639.616 -14514964.658 20305107.161)
  (-18335582.591 -11640868.305 15028599.071)
  (-2077142.705 -20987755.987 -15879741.196)
  (-4957166.885 -23306741.039 12039027.096)
  (17977519.820 -13089823.312 14331151.065)
  (9682727.508 -24060519.485 3985404.530))
;;The distance of the satellite to locate points ρj
'(24658975.31743
   22964286.41228
   21338550.64536
   23606547.29359
   24263298.50401
   20758264.10823
   21847468.81689
   20352077.19349)


[chlh_jd]

Matlab result is (9.613338291042342e+005   -5.674076369901314e+006    2.740537661301808e+006)
Here's my codes ,
Any suggestions are welcome

Code: [Select]

;; Overdetermined Equations : GX=b
;; G=(giu)m×n，m>n
;; Residuals r=b-GX , 2-norm reaches its minimum value
;;
;; E.G.
;; ρj=Sqrt ((X-Xj)^2+(Y-Yj)^2+(Z-Zj)^2)
;;
;;  Satellite coordinates (Xj Yj Zj) =
;|
'((16414028.668 660383.618 20932036.907)
  (16896800.648 -18784061.365 -7418318.856)
  (9339639.616 -14514964.658 20305107.161)
  (-18335582.591 -11640868.305 15028599.071)
  (-2077142.705 -20987755.987 -15879741.196)
  (-4957166.885 -23306741.039 12039027.096)
  (17977519.820 -13089823.312 14331151.065)
  (9682727.508 -24060519.485 3985404.530))
|;
;; ρj =
;|
 '(24658975.31743
   22964286.41228
   21338550.64536
   23606547.29359
   24263298.50401
   20758264.10823
   21847468.81689
   20352077.19349)
   |;
;; Suppose my position r0 (x0 y0 z0)
;; Use Taylor Series Expand The right of the model
;; ljδx+mjδy+njδz+b = Lj ----------(1)
;; In the formula (1) :
;;      lj = (xj-x0)/ Rj~
;;      mj = (yj-y0)/ Rj~
;;      nj = (zj-z0)/ Rj~
;;      δx = x - x0 , δy = y - y0 , δz = z - z0
;;      Lj= Rj~ - ρj 
;;      Rj~ = Sqrt ((X0-Xj)^2+(Y0-Yj)^2+(Z0-Zj)^2)
;;  Rewrite the formula (1) Equation ==>
;;  GX = L   -----------------(2)
;;  In the formula (2) :
;;  X = [δx δy δz b]^T
;;  L = [ L1 ... Ln]^T
;;  G =
;|
 [ l1 m1 n1 -1
   l2 m2 n2 -1
       ...   
   ln mn nn -1]
|;
;; X = (G^T×G)^-1×G^T L

(defun solve:Satellites:position  (sl rl eps / rjl lj ssl G X x0 tor i)
  (setq tor 1e99
X0  (list 0. 0. 0.)
i 0)
  (while (and (> tor eps) (< i 1000))
    (setq rjl (mapcar (function (lambda (a)
  (distance a x0)))
      sl))
    (setq Lj (mapcar (function -) rjl rl))
    (setq ssl
   (mapcar (function (lambda (a b)
       (mapcar (function (lambda (a)
   (/ a b)))
       (mapcar (function -) a x0)
       )))
   sl
   rjl))
    (setq G (mapcar (function (lambda (a)
(append a (list -1.))))
    ssl))   
    (if (not (setq X (butlast (->EQ^++ G Lj))))     
      (setq X (mapcar (function -) (mapcar (function (lambda (a) (* a 2.))) X0))))
    (setq X0 (mapcar (function +) X X0))
    (setq tor (apply (function min) (mapcar (function abs ) X)))
    (setq i (1+ i)))
  (if (= i 1000)
    (princ "\n**Warning***: while loop has reached the upper limit , Please recheck the result !"))
  X0
  )
;;
(defun ->EQ^++ (Gl bl / m mt -mt)
  (setq m  ([trp] Gl)
mt ([*] m Gl))
  (if (setq -mt ([inv] mt))
    (mxv ([*] -mt m) bl)
    )
  )
;;; Use function -----------------------------
(defun butlast(a)
  (reverse (cdr(reverse a)))
  )
;;
;;; mxv apply a transformation matrix to a vector -vladimir
;;; nesterovsky-
(defun mxv(m v)(mapcar(function(lambda (r)(vxv r v)))m))
;;
;;; gile-mxm multiply two matrices -vladimir nesterovsky-
(defun [*](m q)(mapcar(function (lambda (r)(mxv ([trp] q) r)))m))
;;
;;;transpose matrix
(defun [trp] (a) 
  (apply (function mapcar) (cons (function list) a))
)
;;;
;;; gile-inverse-matrix (gile) 2009/03/17
;;; uses the gauss-jordan elimination method to calculate the inverse
;;; matrix of any dimension square matrix
;;;
;;; argument : a square matrix
;;; return : the inverse matrix (or nil if singular)
(defun [inv] (mat / col piv row res)
  (setq mat (mapcar (function (lambda (x1 x2)
(append  x1  x2))) mat([I] (length mat))))
  (while mat
    (setq col (mapcar(function(lambda (x)(abs (car x))))mat))
    (repeat (vl-position(apply(function max)col)col)
      (setq mat (append (cdr mat) (list (car mat)))))   
    (if (equal (setq piv (caar mat))0.0 1e-14)
      (setq mat nil  res nil)     
      (setq piv (/ 1.0 (caar mat))
    row (mapcar(function(lambda (x)(* x piv)))(car mat))
    mat (mapcar (function(lambda (r / e)
     (setq e (car r))(cdr (mapcar(function(lambda (x n)
       (- x (* n e))))r row))))  
  (cdr mat))
    res (cons (cdr row)
      (mapcar(function(lambda (r / e)
   (setq e (car r))(cdr(mapcar(function(lambda (x n)
     (- x (* n e))))r row))))res))
      )))
  (reverse res)
)
;;;--------------------------------------------------------------

result : ("961333.82910424" "-5674076.36990132" "2740537.66130181")

Code: [Select]

(princ
  (mapcar (function (lambda (x)
      (rtos x 2 8)))
  (solve:Satellites:position
    '((16414028.668 660383.618 20932036.907)
      (16896800.648 -18784061.365 -7418318.856)
      (9339639.616 -14514964.658 20305107.161)
      (-18335582.591 -11640868.305 15028599.071)
      (-2077142.705 -20987755.987 -15879741.196)
      (-4957166.885 -23306741.039 12039027.096)
      (17977519.820 -13089823.312 14331151.065)
      (9682727.508 -24060519.485 3985404.530))
    '(24658975.31743 22964286.41228 21338550.64536
      23606547.29359 24263298.50401 20758264.10823
      21847468.81689 20352077.19349)
    1e-8)))


[chlh_jd]

I think that such problems can be written in a common format, and its essence should be a multi-equation least squares method Newton iteration .
Did any one wrote a related function ?

[chlh_jd]

Today , I found Evgeniy has post the Least squares Solution though Cholesky decomposition of matrices ,  two year's ago  (However it can solve only Once overdetermined equations).

http://www.theswamp.org/index.php?topic=32478.0

That 's function is like LeeMac said 'Absolutely brilliant' .