a sketch of using linear relations for LQR control

optimal_relation.hs

-- state of an oscillator
data SHOState = X | P deriving (Show, Enum, Bounded, Eq, Ord)
data Control = F  deriving (Show, Enum, Bounded, Eq, Ord)
-- Costate newtype wrapper
newtype Co a = Co a deriving (Show, Enum, Bounded, Eq, Ord)

type M = Matrix Double
dynamics :: forall x u. (BEnum x, BEnum u) =>  Matrix Double -> Matrix Double ->  
  HLinRel (Either x u) x
dynamics a b =  HLinRel (a ||| b ||| -i )  (vzero cx) where
  cx = card @x
  cu = card @u
  i = ident cx

initial_cond :: forall x. BEnum x => Vector Double-> HLinRel Void x
initial_cond x0 =  HLinRel i x0 where
  cx = card @x
  i = ident cx

valueUpdate :: forall x l. (BEnum x, BEnum l) =>  M -> M -> HLinRel (Either x l) l
valueUpdate a q = HLinRel ((tr a) ||| q ||| i)  (vzero cl) where
  cl = card @l
  i = ident cl  

optimal_u :: forall u l. (BEnum u, BEnum l) => 
     M -> M -> HLinRel u l
optimal_u r b = HLinRel (r ||| tr b) (vzero cu) where
  cu = card @u
 
step :: forall x u l. (BEnum x, BEnum u, BEnum l) => M -> M -> M -> M 
        -> HLinRel (Either x l) (Either x l)
step a b r q = 
  f5 <<< f4 <<< hassoc' <<< f3 <<< f2 <<< hassoc <<< f1 where
  f1 :: HLinRel (Either x l) (Either (Either x x) l)
  f1 = first hdup
  f2 :: HLinRel (Either x (Either x l)) (Either x l)
  f2 = second (valueUpdate a q)
  f3 ::  HLinRel (Either x l) (Either x (Either l l))
  f3 = second hdup
  f4 ::  HLinRel (Either (Either x l) l) (Either (Either x u) l)
  f4 = first (second (hconverse (optimal_u r b)))
  f5 ::  HLinRel (Either (Either x u) l) (Either x l)
  f5 = first (dynamics a b)

-- iterate (hcompose (step a b r q)) :: [HLinRel (Either x l) (Either x l)]