gistfile1.hs

{-# LANGUAGE CPP, TemplateHaskell, MultiParamTypeClasses, FlexibleInstances, FlexibleContexts, UndecidableInstances, DataKinds, InstanceSigs, RankNTypes, NamedFieldPuns#-}
-----------------------------------------------------------------------------
--
-- Module      :  System
-- Copyright   :
-- License     :  AllRightsReserved
--
-- Maintainer  :  Sophie Taylor
-- Stability   :  Non-existant
-- Portability :  Tested on GHC 7.6.1
--
-- |
--
-----------------------------------------------------------------------------

module System (
    System, NoiseCharacteristics, SystemTimeCharacteristics, SystemType,c2d,SDERHS,simulate,toStateVariableHistory,testPlot
) where

import Numeric.LinearAlgebra
import Numeric.GSL
import Numeric.LinearAlgebra.Util
import Data.Maybe
import Graphics.Plot
type SDERHS x u n =  (Num (Vector x), Num (Vector u), Num (Vector n))   (Vector x  Vector u  Vector n  Vector x)

data NoiseCharacteristics = Ideal | Normal { processNoiseVariance,measurementNoiseVariance   Matrix Double}

generateTestPlot dT n =
 toStateVariableHistory $ toFullStateHistory $ take n $ simulate testODE Ideal (1|>[1  Double]) dT (repeat (1|>[0  Double]))

testPlot = generateTestPlot 0.01 300
testFunc ∷  SDERHS Double Double Double
testFunc t x _ _ = scale (-1) x
toFullStateHistory = foldr ( (t, (x, u, n)) (ts,xs,us,ns)   (t:ts,x:xs,u:us,n:ns)) ([],[],[],[])

toStateVariableHistory (ts, xs, us, ns) =  (ts:) $ toLists $ trans $ fromLists $ map (toList) xs
     



simulate   (Floating (Vector x), Container Vector x) SDERHS x x x  -- The RHS of the SDE
            NoiseCharacteristics 
            Vector x
            Double  --Timestep
            [Vector x] --Control input
            [(Double,(Vector x,Vector x,Vector x))]
simulate f Ideal initial deltaT inputHistory
    = zip time (zipWith ( (a,b) c (a,b,c))     simulated  (repeat (constant 0 1))) where
                                        time = [0,deltaT]
                                        simulated = scanl rk4Step (initial, head inputHistory) (tail inputHistory)
                                        dT = realToFrac deltaT
                                        rk4Step (x0,_) control = (x1,control) where
                                         x1 = x0 +
                                           scale (1.0/6.0) (k1 + (scale 2 k2) + (scale 2 k3) + k4) where
                                            k1 = scale dT (f x0 control 0)
                                            k2 = scale dT (f (x0 + scale 0.5 k1) control 0)
                                            k3 = scale dT (f (x0 + scale 0.5 k2) control 0)
                                            k4 = scale dT (f (x0 + k3) control 0)