[irteus/irtdyna.l] Add riccati equation solver based on Arimoto Potter method

irteus/irtdyna.l

@@ -1180,16 +1180,27 @@
 ;;; riccati equation class
 ;;;  solve steady solution of riccati equation
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+;;; Sample
+;;; (send (instance riccati-equation :init AA bb cc QQ RR) :solve)
+;; irteusgl$ (send (instance riccati-equation :init (make-matrix 4 4 '((0 0 1 0) (0 0 0 1) (34.67763 0 0 0) (-679.76829 0 0 0))) (make-matrix 4 1 '((0) (0) (-1.27842) (32.73032))) (make-matrix 2 4 '((1 0 0 0) (0 1 0 0))) 4 7) :solve)
+;; (#2f((1.757195e+05 0.0 0.0 0.0) (0.0 4.0 0.0 0.0) (0.0 0.0 1.757195e+05 0.0) (0.0 0.0 0.0 4.0)) #2f((-27.0313 0.0 0.0 0.0)))
 (defclass riccati-equation
   :super propertied-object
   :slots (A b c P Q R K
             A-bKt R+btPb-1 ;; for buffer
             ))
 (defmethod riccati-equation
-  (:init (AA bb cc QQ RR) 
+  (:init (AA bb cc QQ RR)
+    "Initialize riccati-equation.
+     A, B, C are state eq matrices.
+     q, r are weighting parameters (scalar) of LQR evaluation function."
     (setq A AA b BB c CC Q QQ R RR)
     (setq P (make-matrix (array-dimension c 1) (array-dimension c 1))))
   (:solve ()
+    "Solve riccati-equation.
+     Return (list P K).
+     P is solution (matrix) of riccati equation based on iterative method.
+     K is LQR state feedback gain (matrix):  u = -Kx = -(inv(R)*trans(B)*P)x."
     (let (diffP
 	  (At (transpose A))
 	  (bt (transpose b))
@@ -1214,6 +1225,76 @@
       ))
   )
 
+;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+;;; riccati equation class by Arimoto Potter method
+;;;  solve riccati equation based on eigenvalue decomposition
+;;;   http://www.humachine.fr.dendai.ac.jp/lec/SDRE_%E3%83%86%E3%82%AF%E3%83%8E%E3%82%BB%E3%83%B3%E3%82%BF%E8%AC%9B%E6%BC%9405.pdf
+;;;   http://www.maizuru-ct.ac.jp/control/kawata/study/book3/matlab/chap08/doc/sec842.html
+;;;   http://stlab.ssi.ist.hokudai.ac.jp/yuhyama/lecture/linearsys/st8-8up.pdf
+;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+;;; Sample
+;;; (send (instance riccati-equation-arimoto-potter-method :init AA BB QQ RR) :solve)
+;; irteusgl$ (send (instance riccati-equation-arimoto-potter-method :init (make-matrix 4 4 '((0 0 1 0) (0 0 0 1) (34.67763 0 0 0) (-679.76829 0 0 0))) (make-matrix 4 1 '((0) (0) (-1.27842) (32.73032))) (make-matrix 4 4 '((4 0 0 0) (0 4 0 0) (0 0 4 0) (0 0 0 4))) (make-matrix 1 1 '((7)))) :solve)
+;; (#2f((35705.6 314.265 12343.3 441.199) (314.265 7.02923 111.06 4.17625) (12343.3 111.06 4331.8 156.495) (441.199 4.17625 156.495 5.82846)) #2f((-191.33 -0.755929 -59.3904 -1.3284)))
+(defclass riccati-equation-arimoto-potter-method
+  :super propertied-object
+  :slots (A B Q R
+          P K
+          ))
+(defmethod riccati-equation-arimoto-potter-method
+  (:init (AA BB QQ RR)
+    "Initialize riccati-equation-arimoto-potter-method.
+     A, B are state eq matrices.
+     Q, R are weighting matrices of LQR evaluation function."
+    (setq A AA B BB Q QQ R RR))
+  (:solve ()
+    "Solve riccati-equation-arimoto-potter-method.
+     Return (list P K).
+     P is solution (matrix) of riccati equation based on Arimoto Potter method (eigenvalue decomposition method).
+     K is LQR state feedback gain (matrix):  u = -Kx = -(inv(R)*trans(B)*P)x."
+    (let* ((n (car (array-dimensions A))) ;; dimensions n
+           (H11 A)
+           (H12 (m* (m* (scale-matrix -1 B) (inverse-matrix R)) (transpose B)))
+           (H21 (scale-matrix -1 Q))
+           (H22 (scale-matrix -1 (transpose A)))
+           (Hamilton (concatenate-matrix-column
+                      (concatenate-matrix-row H11 H12)
+                      (concatenate-matrix-row H21 H22))) ;; Hamilton matrix (2n*2n size matrix)
+           Lambda V
+           V-negative-eigen-real V-negative-eigen-imag V-negative-eigen
+           V1-real V1-imag V1
+           V2-real V2-imag V2)
+      ;;;;; Eigenvalue Decomposition for Hamilton Matrix (to solve Eigen Values and Eigen Vectors) ;;;;;
+      (setq Lambda (car (eigen-decompose-complex Hamilton))) ;; all eigen values '(Re(Lambda) Im(Lambda)) = '(#f(2n size float-vector) #f(2n size float-vector))
+      (setq V (cadr (eigen-decompose-complex Hamilton))) ;; matrix composed of counterpart eigen vectors '(Re(V) Im(V)) = '(#2f(2n*2n size matrix) #2f(2n*2n size matrix))
+      ;;;;; Generate Matrix Composed of Eigen Vectors which have "Negative Real Part" Eigen Values (2n*n size matrix) ;;;;;
+      (dotimes (i (* 2 n))
+        (if (< (elt (car Lambda) i) 0) ;; "negative real part" eigen values only
+            (progn
+              (push (matrix-column (car V) i) V-negative-eigen-real)
+              (push (matrix-column (cadr V) i) V-negative-eigen-imag))
+          )
+        )
+      (setq V-negative-eigen (mapcar #'(lambda (x) (transpose (apply 'matrix (reverse x)))) (list V-negative-eigen-real V-negative-eigen-imag)))
+      ;; (print V-negative-eigen)
+      ;;;;; Split V-negative-eigen into Two Rows ;;;;;
+      (setq V1-real (m* (concatenate-matrix-row (unit-matrix n) (make-matrix n n)) (car V-negative-eigen))) ;; upper row matrix of Re(V-negative-eigen),  Re(V1) = [E O] * Re(V-negative-eigen)
+      (setq V1-imag (m* (concatenate-matrix-row (unit-matrix n) (make-matrix n n)) (cadr V-negative-eigen))) ;; upper row matrix of Im(V-negative-eigen),  Im(V1) = [E O] * Im(V-negative-eigen)
+      (setq V1 (list V1-real V1-imag))
+      (setq V2-real (m* (concatenate-matrix-row (make-matrix n n) (unit-matrix n)) (car V-negative-eigen))) ;; lower row matrix of Re(V-negative-eigen),  Re(V2) = [O E] * Re(V-negative-eigen)
+      (setq V2-imag (m* (concatenate-matrix-row (make-matrix n n) (unit-matrix n)) (cadr V-negative-eigen))) ;; lower row matrix of Im(V-negative-eigen),  Im(V2) = [O E] * Im(V-negative-eigen)
+      (setq V2 (list V2-real V2-imag))
+      (setq P (m*-complex V2 (inverse-matrix-complex V1))) ;; solution P of riccati equation:  P = V2 * inv(V1)
+      (unless (eps= (norm (array-entity (cadr P))) 0.0) ;; If Im(P) \neq O
+        (warn ";; !!WARNING!! : imaginary part of riccati solution Im(P) should be zero matrix.~%")
+        (warn ";; Im(P) = ~a~%" (cadr P))
+        )
+      (setq P (car P)) ;; P = Re(P), Im(P) = O
+      (setq K (m* (m* (inverse-matrix R) (transpose B)) P)) ;; LQR state feedback gain K:  u = -Kx = -(inv(R)*trans(B)*P)x
+      (return-from :solve (list P K))
+      ))
+  )
+
 ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
 ;;; preview-controller class
 ;;;  A, b, c -> system configuration matrices

irteus/test/riccati-equation-arimoto-potter-method-test.l

@@ -0,0 +1,125 @@
+(require :unittest "lib/llib/unittest.l")
+
+(init-unit-test)
+
+(defun eps-matrix= (m1 m2 &optional (eps *epsilon*))
+   (eps= (distance (array-entity m1) (array-entity m2)) 0.0 eps))
+
+(defun set-ABQR-sample1 ()
+  ;; Hamilton matrix of this (A, B, Q, R) has only real number eigenvalue
+  ;; About Hamilton matrix : http://stlab.ssi.ist.hokudai.ac.jp/yuhyama/lecture/linearsys/st8-8up.pdf
+  (let* (A B Q R)
+    (setq A (matrix
+             (float-vector 0 1)
+             (float-vector -10 -1)))
+    (setq B (matrix
+             (float-vector 0)
+             (float-vector 1)))
+    (setq Q (diagonal (float-vector 300 60)))
+    (setq R (diagonal (float-vector 0.8)))
+    (list A B Q R)
+    ))
+
+(defun set-ABQR-sample2 ()
+  ;; Hamilton matrix of this (A, B, Q, R) has complex number eigenvalue
+  ;; About Hamilton matrix : http://stlab.ssi.ist.hokudai.ac.jp/yuhyama/lecture/linearsys/st8-8up.pdf
+  ;; Sample data : https://qiita.com/taka_horibe/items/5d053cdaaf4c886d27b5
+  (let* (A B Q R)
+    (setq A (matrix
+             (float-vector 0 1 0 0)
+             (float-vector 0 -15 10 0)
+             (float-vector 0 0 0 1)
+             (float-vector 0 0 0 -15)))
+    (setq B (matrix
+             (float-vector 0)
+             (float-vector 10)
+             (float-vector 0)
+             (float-vector 1)))
+    (setq Q (diagonal (float-vector 1 0 1 0)))
+    (setq R (diagonal (float-vector 1)))
+    (list A B Q R)
+    ))
+
+(defun lqr-two-wheeled-inverted-pendulum (&key
+                                          (m-w 4.0) ;; mass of wheels [kg]
+                                          (m-b 59.9271) ;; mass of base [kg]
+                                          (r-w 0.0447) ;; radius of wheels [m]
+                                          (L 0.905396) ;; distance between CoM of base and wheels [m]
+                                          (I-w 0.002644) ;; inertia of wheels [kg*m^2]
+                                          (I-b 11.3257) ;; inertia of base [kg*m^2]
+                                          (g 9.8) ;; gravitational acceleration [m/s^2]
+                                          (Q (diagonal (float-vector 10 0.8 90 5))) ;; weight parameter Q of LQR control
+                                          (R (diagonal (float-vector 7))) ;; weight parameter R of LQR control
+                                          )
+  ;; Inverted pendulum model refers to the following paper
+  ;; "TWIMP: Two-Wheel Inverted Musculoskeletal Pendulum as a Learning Control Platform in the Real World with Environmental Physical Contact" (published in Humanoids2018)
+  ;; https://ieeexplore.ieee.org/document/8624923
+  (let* ((a (+ (* (+ m-b m-w) (expt r-w 2)) I-w))
+         (b (* m-b r-w L))
+         (c (+ (* m-b (expt L 2)) I-b))
+         (d (* m-b g L))
+         EE A0 B0 AA BB)
+    (setq EE (matrix
+              (float-vector 1 0 0 0)
+              (float-vector 0 1 0 0)
+              (float-vector 0 0 (+ a (* 2 b) c) (+ a b))
+              (float-vector 0 0 (+ a b) a)))
+    (setq A0 (matrix
+              (float-vector 0 0 1 0)
+              (float-vector 0 0 0 1)
+              (float-vector d 0 0 0)
+              (float-vector 0 0 0 0)))
+    (setq B0 (matrix
+              (float-vector 0)
+              (float-vector 0)
+              (float-vector 0)
+              (float-vector 1)))
+    (setq AA (m* (inverse-matrix EE) A0))
+    (setq BB (m* (inverse-matrix EE) B0))
+    (send (instance riccati-equation-arimoto-potter-method :init AA BB Q R) :solve)
+    ))
+
+(deftest riccati-equation-arimoto-potter-method-test
+  (let* ((ABQR1 (set-ABQR-sample1))
+         (solution-ABQR1-arimoto-potter (send (instance riccati-equation-arimoto-potter-method :init (elt ABQR1 0) (elt ABQR1 1) (elt ABQR1 2) (elt ABQR1 3)) :solve))
+         (solution-ABQR1-matlab-lqr (list
+                                     #2f((165.9973 9.4356)
+                                         (9.4356 7.1835)) ;; riccati solution P
+                                     #2f((11.7945 8.9794)) ;; feedback gain K
+                                     ))
+         ;;
+         (ABQR2 (set-ABQR-sample2))
+         (solution-ABQR2-arimoto-potter (send (instance riccati-equation-arimoto-potter-method :init (elt ABQR2 0) (elt ABQR2 1) (elt ABQR2 2) (elt ABQR2 3)) :solve))
+         (solution-ABQR2-matlab-lqr (list
+                                     #2f((1.57106564 0.10038768 0.00582396 -0.00387677)
+                                         (0.10038768 0.00655059 0.00038843 -0.00025543)
+                                         (0.00582396 0.00038843 15.06666504 0.99999249)
+                                         (-0.00387677 -0.00025543 0.99999249 0.06652974)) ;; riccati solution P
+                                     #2f((1.000000 0.065250 1.003877 0.063975)) ;; feedback gain K
+                                     ))
+         ;;
+         (solution-InvPend-arimoto-potter (lqr-two-wheeled-inverted-pendulum))
+         (solution-InvPend-matlab-lqr (list
+                                       #2f((31440.35317 131.47107 10757.90072 381.66021)
+                                           (131.47107 2.60486 46.41979 1.74082)
+                                           (10757.90072 46.41979 3759.68634 134.96830)
+                                           (381.66021 1.74082 134.96830 5.03633)) ;; riccati solution P
+                                       #2f((-180.17687 -0.33806 -55.55667 -1.10076)) ;; feedback gain K
+                                       ))
+         )
+    ;;;;; Sample1: ABQR1 ;;;;;
+    (assert (eps-matrix= (car solution-ABQR1-arimoto-potter) (car solution-ABQR1-matlab-lqr)) "[ABQR1] compare riccati solution P between riccati-equation-arimoto-potter-method and matlab-lqr")
+    (assert (eps-matrix= (cadr solution-ABQR1-arimoto-potter) (cadr solution-ABQR1-matlab-lqr)) "[ABQR1] compare feedback gain K between riccati-equation-arimoto-potter-method and matlab-lqr")
+
+    ;;;;; Sample2: ABQR2 ;;;;;
+    (assert (eps-matrix= (car solution-ABQR2-arimoto-potter) (car solution-ABQR2-matlab-lqr)) "[ABQR2] compare riccati solution P between riccati-equation-arimoto-potter-method and matlab-lqr")
+    (assert (eps-matrix= (cadr solution-ABQR2-arimoto-potter) (cadr solution-ABQR2-matlab-lqr)) "[ABQR2] compare feedback gain K between riccati-equation-arimoto-potter-method and matlab-lqr")
+
+    ;;;;; Sample: InvPend ;;;;;
+    (assert (eps-matrix= (car solution-InvPend-arimoto-potter) (car solution-InvPend-matlab-lqr)) "[InvPend] compare riccati solution P between riccati-equation-arimoto-potter-method and matlab-lqr")
+    (assert (eps-matrix= (cadr solution-InvPend-arimoto-potter) (cadr solution-InvPend-matlab-lqr)) "[InvPend] compare feedback gain K between riccati-equation-arimoto-potter-method and matlab-lqr")
+    ))
+
+(eval-when (load eval)
+  (run-all-tests)
+  (exit))