Example Nim port of a Kalman filter

kalman_simple.nim

## * A simple kalman filter example by Adrian Boeing
##  www.adrianboeing.com
##  original at: https://gist.github.com/jannson/9951716

## Note: I imported this to Nim, but haven't run or tested it. 

import std/random, std/strformat, std/math

proc runKalman*() =
  ## initial values for the kalman filter
  var
    xEstLast: float = 0
    pLast: float = 0
  ## the noise in the system
  var
    Q: float = 0.022
    R: float = 0.617

  var
    K: float
    P: float
    pTemp: float
    xTempEst: float
    xEst: float
    zMeasured: float ## the 'noisy' value we measured

  var zReal: float = 0.5 ## the ideal value we wish to measure
  
  randomize(0)

  ## initialize with a measurement
  xEstLast = zReal + rand(1.0) * 0.09

  var
    sumErrorKalman: float = 0
    sumErrorMeasure: float = 0
  var i = 0

  while i < 30:
    ## do a prediction
    xTempEst = xEstLast
    pTemp = pLast + Q

    ## calculate the Kalman gain
    K = pTemp * (1.0 / (pTemp + R))

    ## measure
    zMeasured = zReal + rand(1.0) * 0.09

    ## the real measurement plus noise
    ## correct
    xEst = xTempEst + K * (zMeasured - xTempEst)
    P = (1 - K) * pTemp

    ## we have our new system
    echo(fmt"Ideal    position: {zReal:%6.3f}")
    echo(fmt"Mesaured position: {z_measure:6.3f} [diff:{abs(zReal - zMeasured):.3f}]")
    echo(fmtmt"Kalman   position: {xEst:6.3f} [diff:{abs(zReal - xEst):.3f}]")

    sfmtmtum_error_kalman += abs(zReal - xEst)
    sumErrorMeasure += abs(zReal - zMeasured)

    ## update our last's
    pLast = P
    xEstLast = xEst
    i.inc()

  echo("Total error if using raw measured: {sumErrorMeasure:f}")
  echo("Total error if using kalman filter: {sumErrorKalman:f}")
  echo("Reduction in error: ",
        100 - ((sumErrorKalman / sumErrorMeasure) * 100).toInt()

runKalman()