System.hs

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

module System (
    System, NoiseCharacteristics, SystemTimeCharacteristics, SystemType,SDERHS,simulate
) where

import Numeric.LinearAlgebra
import Numeric.GSL
import Numeric.LinearAlgebra.Util
import Data.Maybe

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}

possiblyComplex :: Double -> a
possiblyComplex t = undefined
toCD:: Double -> Complex Double
toCD = realToFrac
leaveThatShitAlone:: Double -> Double
leaveThatShitAlone = id
{-#RULES "ConvertTimeToComplexDouble" possiblyComplex = toCD#-}
{-#RULES "Don'tFuckAboutWithTime" possiblyComplex = leaveThatShitAlone#-}


simulate :: (Num (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, undefined) inputHistory
                                        dT = possiblyComplex 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)