Kalman filter

For small values of 9#9 we have that 11#11, which allow us to write the dynamical system associated with this controller as

  

12#12 = 13#13 (5.3)

A = 14#14 (5.4)

We can obtain a discrete system as

15#15 = 16#16 (5.5)

where 17#17. Moreover, we assume that there is some noise in the process , w_t, with constant distribution process G. Thus, the dynamical model of our Kalman filter is defined by

15#15 = 18#18 (5.6)

As for the measurement model of the Kalman filter, we assume that e and 9#9 can be observed, and consequently the reading at time t, z_t, obeys the eequation

z_t = Ix_t + v_t (5.7)

where v_t is the measurement noise, which we assume it has constant covariance R.

Now that we have defined the dynamical and measurement models of the Kalman filter, we proceed to define the corresponding code.^5.2

;; boundary following kalman filter
(define-class <bf-kalman-filter> (<kalman-filter>)
 (constant-part-transistion-matrix)
 (initial-state-covariance)
 (process-noise-constant-covariance)
 (measurement-noise-constant-covariance)
)

(define (boundary-following::set-kalman-filter ke kth v)

  ;;; set the value of global constants needed for the calculation
  ;;; of the kalman filter
  ;;;

  (let ((kf (make <bf-kalman-filter> dimension: 2)))

    ;;; set process model

    (set-constant-part-transition-matrix! kf (matrix:create 2 2 0))
    (matrix:set! (constant-part-transition-matrix kf) 1 1 (- kth))
    (matrix:set! (constant-part-transition-matrix kf)
         1 2 (/ (- ke) v))
    (matrix:set! (constant-part-transition-matrix kf) 2 1 v)

    (set-state-transition-matrix! kf
          boundary-following::transition-matrix)                                  
    (set-process-noise-matrix! kf
          boundary-following::process-noise-matrix)

    (set-process-noise-constant-covariance! kf (matrix:create 2 2 0))
    (matrix:set! (process-noise-constant-covariance kf)
         1 1 (sqr (deg->rad 2)))
    (matrix:set! (process-noise-constant-covariance kf) 2 2 (sqr 0.025))
    (set-process-noise-covariance! kf
         boundary-following::process-noise-covariance)

    ;;; set measurement model

    (set-measurement-matrix! kf
         boundary-following::measurement-matrix)

    (set-measurement-noise-constant-covariance! kf (matrix:create 2 2 0))
    (matrix:set! (measurement-noise-constant-covariance kf)
         1 1 (sqr (deg->rad 3)))
    (matrix:set! (measurement-noise-constant-covariance kf)2 2 (sqr 0.1))

    (set-measurement-noise-covariance! kf
         boundary-following::measurement-noise-covariance)

    kf
  )
)

(define (boundary-following::transition-matrix kf delta_t)
 ;;; I + A*delta_t
  (matrix:+ (matrix:identity 2) 
     (matrix:* (constant-part-transition-matrix kf) delta_t))
)

(define (boundary-following::process-noise-matrix kf delta_t)
  (matrix:identity 2)
)

(define (boundary-following::measurement-matrix kf delta_t)
  ;;; Identity
  (matrix:identity 2)
)

(define (boundary-following::process-noise-covariance kf delta_t)
  ;;; constant 
  (process-noise-constant-covariance kf)
)

(define (boundary-following::measurement-noise-covariance kf delta_t)
  ;;; constant
   (measurement-noise-constant-covariance kf)
)

(define (boundary-following::make-reading error theta)
  (list->matrix (list theta error))
)