Boundary following Controller

Let's consider the implementation of the controller

 7#7 (5.1)

which defines a control law for specifying the value of the controlled variable 8#8 as a function of the observed variables e, 9#9 and v. Moreover, the constraint

 10#10 (5.2)

guarantees that the system is critically damped.

The code below implements the controller described above.^5.1

(set! *mc-handle* (start (make <system-handle>)))

(define-splat (bf-controller robot setpoint ke kth v)
 (local-vars (e 0) (th 0) (init-state-covariance #f) (omega 0)
             (kf (boundary-following::set-kalman-filter ke kth v))
 )
 (finalizer (halt robot))

 (method 
    #t

    (define (current-e-theta)
       (bind ((ang r (min-free-space robot (deg->rad -120) (deg->rad -60))))
          (set! e (- r setpoint))
          (set! th (- (deg->rad -90) ang ))))

    ;; set initial estimates of error and thetha and initialize the 
    ;; kalman filter

    (set! init-state-covariance  (matrix:create 2 2 0))
    (matrix:set! init-state-covariance 1 1 (sqr (deg->rad 2)))
    (matrix:set! init-state-covariance 2 2 (sqr 0.2))
    (current-e-theta)
    (start kf (boundary-following::make-reading e th) init-state-covariance)
    
    ;; follow the wall

    (let loop ()
      (current-e-theta)
      (project kf (boundary-following::make-reading e th))
      (set! omega 
            (* (/ -1.0 (max 0.01 v))
               (+ (* ke (matrix:ref (state kf) 2 1)) 
                  (* kth v (matrix:ref (state kf) 1 1)))))
      (trace *mc-handle*
        (format #f "reading: (~a,~a) --> state: (~a, ~a) --> omega = ~a"
                e (rad->deg th) 
                (matrix:ref (state kf) 2 1) 
                (rad->deg (matrix:ref (state kf) 1 1))
                (rad->deg omega)))
      (check-and-possibly-quit self)
      (set-drive-and-turn robot v omega)
      (loop))
  )
)

The function boundary-following::set-kalman-filter defines the Kalman filter associated with the controller (see next section). The value of e and 9#9 can be calculated directly from the robot readings. However, this calculation might be erroneous and we use a Kalman filter to filter out errors in it.