Control Laws

• ImProve Home

These examples demonstrate how to design common controls system digital signal processing structures with ImProve.

PID Controller

The PID controller is most common control algorithm. In its most basic form, it consists of a proportion, integral, and derivative path, hence the name.

Derivative Block

Before we build a PID function, we need to assemble a few low level building blocks. First we need a ratetaker to approximate the derivative of a signal. As arguments, the 'derivative' function will take a name (for creating a new variable scope), the sample time, and the input signal; and will return the signal derivative:

derivative :: Name -> Float -> E Float -> Stmt (E Float)
derivative name sampleTime a = name -| do
  -- Create a variable to remember the last sample.
  a1 <- float "a1" 0

  -- Create a variable to hold the result.
  x <- float "x" 0

  -- Compute the derivative approximation.
  x <== (a - ref a1) /. sampleTime

  -- Save the current input for next time.
  a1 <== a

  -- Return the result.
  return $ ref x

Very often we declare variables only to immediately assign them values, as is the case with 'x'. ImProve has a family of functions (bool', int', float') to do this in one step. So we can rewrite 'derivative' as:

derivative :: Name -> Float -> E Float -> Stmt (E Float)
derivative name sampleTime a = name -| do
  a1 <- float "a1" 0                            -- Create a variable to remember the last sample.
  x <- float' "x" $ (a - ref a1) /. sampleTime  -- Compute the derivative approximation.
  a1 <== a                                      -- Save the current input for next time.
  return $ ref x                                -- Return the result.

A few comments. First, a name is passed into the function. This is used to create a new hierarchical scope with the (-|) operator. A new scope is important because we want to ensure that any local variables names (e.g. 'a1', 'x') do not collide with other variables. As a general rule, when ever you create a reusable component that declares local variables, you'll want to follow this pattern for creating safe hierarchical scopes.

Second, note that 'sampleTime' is a 'Float', not and 'E Float'. Also notice the special division operator (/.), which has a type signature of:

(/.) :: E Float -> Float -> E Float

SMT model checking requires linear expressions. The special multiplication (*.) and division (/.) operators ensure that all ImProve expressions are linear, and thus will be compatible with formal verification.

Integrator Block

The next building block we need is an integrator. We will make a trapezoidal integrator function that follows the same arguments as our 'derivative' block:

integrator :: Name -> Float -> E Float -> Stmt (E Float)
integrator name sampleTime a = name -| do
  a1 <- float "a1" 0  -- A variable to save the last input.
  x  <- float "x"  0  -- A variable to accumulate over time.
  x  <== ref x + (a + ref a1) /. 2 *. sampleTime  -- Compute the integral.
  a1 <== a            -- Save the input for next time.

Closing the Loop

Now that we have integral and derivative blocks in our library, constructing a PID controller is easy:

pid :: Name -> Float -> (Float, Float, Float) -> E Float -> Stmt (E Float)
pid name sampleTime (p, i, d) a = name -| do
  p' <- return $ a *. p
  i' <- integrator "i" sampleTime a
  d' <- derivative "d" sampleTime a
  float' "x" $ p' + i' + d'

And using the 'pid' block is just as easy:

actuatorCommand <- pid "servoLoop" 0.001 (2.3, 0.2, 0.6) (command - feedback)

Parametric FIR Filter

One of the benefits of embedding a DSL in a functional language like Haskell is that you have the power of functional programming at your command. In this example we will leverage some of that power by using recursive functions to build a parametric FIR filter block to add to our growing library. The filter will be parametric in that it will support any number of filter coefficients, as represented as a list of Floats.

Unit Delay

As in our previous example, we'll need a few low level building blocks before we start on the filter itself. First let's build a unit delay (1/z) block:

unitDelay :: AllE a => Name -> E a -> Stmt (E a)  -- Polymorphic for any ImProve type.
unitDelay name a = name -| do
  a1 <- var "a1" zero  -- Variable to hold last input.
  x  <- var "x"  zero  -- Variable to hold result.
  x  <== ref a1        -- Grab current value of unit delay memory.
  a1 <== a             -- Save current input for next cycle.
  return $ ref x       -- Return the result.

Delay Chain

Cascaded chains of unit delays are common in DSP, and we will need one for our FIR filter block. Our 'delayChain' block will take a integer paremeter, which is the number of unit delays in the chain, and will return a list of signals of all the delayed versions of the signal:

delayChain :: AllE a => Name -> Int -> E a -> Stmt [E a]
delayChain name n a = name -| chain 1 a >>= return . (a :)
  where
  -- A recursive function to assemble the delay chain.
  chain :: AllE a => Int -> E a -> Stmt [E a]
  chain i a 
    | i > n = return []
    | otherwise = do
        d  <- unitDelay ("d" ++ show i) a
        ds <- chain (i + 1) d
        return $ d : ds

Complete Filter

With 'unitDelay' and 'delayChain' in our library, the filter instantiates a delay chain then multiplies the taps by the coefficients and sums them up:

firFilter :: Name -> [Float] -> E Float -> Stmt (E Float)
firFilter name coeffs a = name -| do
  ds <- delayChain "delays" (length coeffs - 1) a
  float' "x" $ sum [ d *. c | (d, c) <- zip ds coeffs ]

To use the new filter:

x <- firFilter "channel1Filter" [0.1, 0.2, 0.3, 0.2, 0.1] a