gistfile1

import scala.util.Random


class Kalman(N: Int) {
  private[this] val mbuf = new Array[Double](N)
  private[this] val ebuf = new Array[Double](N)
  private[this] var estimate: Double = _
  private[this] var n = 0L
  private[this] var weight: Double = 0.9

  def measure(m: Double, e: Double) {
    val i = (n%N).toInt
    mbuf(i) = m
    ebuf(i) = e

    if (n == 0)
      estimate = m

    estimate += weight*(m-estimate)
    if (mvar + evar == 0)
      weight = 1D
    else
      weight = mvar / (mvar + evar)
    n += 1
  }

  private[this] def mvar = variance(
    if (n < N) mbuf take n.toInt
    else mbuf
  )

  private[this] def evar = variance(
    if (n < N) ebuf take n.toInt
    else ebuf
  )

  def currentEstimate = estimate

  def variance(samples: Array[Double]): Double = {
    if (samples.size == 1)
      return 0D

    val sum = samples.sum
    val mean = sum / samples.size
    val diff = (samples map { x => (x-mean)*(x-mean) }).sum
    diff/(samples.size-1)
  }

  override def toString = 
    "Kalman<estimate=%f, weight=%f, mvar=%f, evar=%f>".format(estimate, weight, mvar, evar)
}

class KalmanError(N: Int, range: Double) extends Kalman(N) {
  require(range >= 0D && range <1D)
  private[this] val rng = new Random
  
  def measure(m: Double) {
    measure(m, rng.nextGaussian()*range*m)
  }
}


val k = new KalmanError(50, .1)
for (m <- Seq(1.0, 1.2, 3.2, 2.3, 4.4, 3.4, 3.4, 3.4, 3.4, 3.4, 100D, 1D, 1D, 1D, 1D, 1D)) {
  k.measure(m)
  println("updated with %f: %s".format(m, k))
}