;This code written in ANSII Common Lisp by Prof. Robert B. McGhee (mcghee@
;cs.nps.navy.mil) at the Naval Postgraduate School, Monterey, CA93940.
;The test functions t1 through t3 show that 10 cycles of Gauss-Newton 
;iteration should generally be sufficient for quaternion orientation filter
;startup, while test functions t4 through t15 suggest that one or two cycles
;should suffice for tracking. Date of latest update: 18 Aug 00.

(load "D:\\cs4314\\math-routines\\quaternion-functions")

(defvar b (list 0 (- (cos (deg-to-rad 60))) 0 (sin (deg-to-rad 60)))) 

(defun 3-q-prod (q1 q2 q3)
  (quaternion-product q1 (quaternion-product q2 q3)))

(defun X-col-1 (unit-quaternion)
  (let* ((q unit-quaternion) (q-inv (quaternion-inverse q)) 
         (q1 (3-q-prod '(1 0 0 0) k q)) (q2 (3-q-prod q-inv k '(1 0 0 0)))
         (q3 (3-q-prod '(1 0 0 0) b q)) (q4 (3-q-prod q-inv b '(1 0 0 0)))
         (term-1 (vector-add q1 q2)) (term-2 (vector-add q3 q4)))
        (append (rest term-1) (rest term-2))))

(defun X-col-2 (unit-quaternion)
  (let* ((q unit-quaternion) (q-inv (quaternion-inverse q)) 
         (q1 (3-q-prod '(0 -1 0 0) k q)) (q2 (3-q-prod q-inv k '(0 1 0 0)))
         (q3 (3-q-prod '(0 -1 0 0) b q)) (q4 (3-q-prod q-inv b '(0 1 0 0)))
         (term-1 (vector-add q1 q2)) (term-2 (vector-add q3 q4)))
        (append (rest term-1) (rest term-2))))

(defun X-col-3 (unit-quaternion)
  (let* ((q unit-quaternion) (q-inv (quaternion-inverse q)) 
         (q1 (3-q-prod '(0 0 -1 0) k q)) (q2 (3-q-prod q-inv k '(0 0 1 0)))
         (q3 (3-q-prod '(0 0 -1 0) b q)) (q4 (3-q-prod q-inv b '(0 0 1 0)))
         (term-1 (vector-add q1 q2)) (term-2 (vector-add q3 q4)))
        (append (rest term-1) (rest term-2))))

(defun X-col-4 (unit-quaternion)
  (let* ((q unit-quaternion) (q-inv (quaternion-inverse q)) 
         (q1 (3-q-prod '(0 0 0 -1) k q)) (q2 (3-q-prod q-inv k '(0 0 0 1)))
         (q3 (3-q-prod '(0 0 0 -1) b q)) (q4 (3-q-prod q-inv b '(0 0 0 1)))
         (term-1 (vector-add q1 q2)) (term-2 (vector-add q3 q4)))
        (append (rest term-1) (rest term-2))))

(defun X-sub-v-col-1 (q) ;dy/dv1
  (post-multiply (X-matrix q)
                 (quaternion-product q '(0 1 0 0)))) 

(defun X-sub-v-col-2 (q) ;dy/dv2
  (post-multiply (X-matrix q)
                 (quaternion-product q '(0 0 1 0))))

(defun X-sub-v-col-3 (q) ;dy/dv3
  (post-multiply (X-matrix q)
                 (quaternion-product q '(0 0 0 1)))) 

(defun X-matrix (q)
  (transpose (list (X-col-1 q) (X-col-2 q) (X-col-3 q) (X-col-4 q))))

(defun X-sub-v-matrix (q)
  (transpose (list (X-sub-v-col-1 q) (X-sub-v-col-2 q) (X-sub-v-col-3 q))))

(defun computed-measurement (q)
  (let* ((q-inv (quaternion-inverse q))  
         (q1 (3-q-prod q-inv k q)) (q2 (3-q-prod q-inv b q)))
        (append (rest q1) (rest q2))))

(defun normalize-measurement (y)
  (append (normalize-vector (firstn 3 y)) (normalize-vector (nthcdr 3 y))))

(defun random-start (q-true)
  (vector-add q-true (list (- (random .2) .1) (- (random .2) .1)
                           (- (random .2) .1) (- (random .2) .1))))

(defun noise-vector (max-noise)
  (do* ((i 5 (1- i))
        (noise (list (- (random 2) 1))
               (cons (- (random 2) 1) noise)))
       ((zerop i) (scalar-multiply max-noise noise))))
         
(defun noisy-measurement (q max-noise)
  (normalize-measurement (vector-add (computed-measurement q)
                                     (noise-vector max-noise))))
                
(defun delta-v (measurement-vector estimated-q)
  (let* ((q estimated-q) (y0 measurement-vector) (y (computed-measurement q))
         (error (vector-subtract y0 y)) (X (X-sub-v-matrix q))
         (X-trans (transpose X)) 
         (M (matrix-inverse (matrix-multiply X-trans X)))
         (N (matrix-multiply M X-trans)))
        (post-multiply N error)))

(defun best-q (q-start q-true max-noise number-of-GN-cycles)
  (do* ((measurement (noisy-measurement q-true max-noise))
        (q-cap q-start)
        (count (1- number-of-GN-cycles)(1- count))
        (v (delta-v measurement q-cap) (delta-v measurement q-cap))
        (delta-q (quaternion-product q-cap (cons 0 v))
                 (quaternion-product q-cap (cons 0 v)))
        (q-cap (normalize (vector-add q-cap delta-q)) 
               (normalize (vector-add q-cap delta-q)))) 
       ((zerop count) (list q-true q-cap))))

(defun random-q () (normalize (list (- (random 2.0) 1) (- (random 2.0) 1)
                                    (- (random 2.0) 1) (- (random 2.0) 1))))

(defun rms-estimation-error (max-noise number-of-samples GN-depth)
  (do* ((n (1- number-of-samples) (1- n))
        (q-true (random-q) (random-q))
        (q-start (random-start q-true) (random-start q-true))
        (q-cap (second (best-q q-start q-true max-noise GN-depth))
               (second (best-q q-start q-true max-noise GN-depth)))
        (error (vector-subtract q-true q-cap) (vector-subtract q-true q-cap))
        (sum-sq-error (dot-product error error)
                      (+ sum-sq-error (dot-product error error))))
       ((zerop n) (sqrt (/ sum-sq-error number-of-samples)))))

(defun t1 () (best-q (random-q) (random-q) 0.001 1))

(defun t2 () (best-q (random-q) (random-q) 0.001 3))

(defun t3 () (best-q (random-q) (random-q) 0.001 10))

(defun t4 () (rms-estimation-error 0 100 1))
   
(defun t5 () (rms-estimation-error 0 100 2))
   
(defun t6 () (rms-estimation-error 0 100 3))

(defun t7 () (rms-estimation-error .001 100 1))
   
(defun t8 () (rms-estimation-error .001 100 2))
   
(defun t9 () (rms-estimation-error .001 100 3))

(defun t10 () (rms-estimation-error .01 100 1))
   
(defun t11 () (rms-estimation-error .01 100 2))
   
(defun t12 () (rms-estimation-error .01 100 3))   

(defun t13 () (rms-estimation-error .1 100 1))
   
(defun t14 () (rms-estimation-error .1 100 2))
   
(defun t15 () (rms-estimation-error .1 100 3))