kalman.rb

kalman.rb

require "matrix"

# Simple Kalman implementation, with one hidden variable: velocity. 
# For very simple explanation see https://www.udacity.com/course/artificial-intelligence-for-robotics--cs373 (Lession 2)

class Kalman
  attr_accessor :state
  attr_accessor :process_noise
  attr_reader :control
  attr_accessor :model
  attr_reader :readings
  attr_reader :sensor_noise
  attr_reader :identity

  def initialize
    # x = estimate (intial state)
    @state = Matrix[
      [247.0], # Height,
      [0.0]  # Velocity
    ]

    # P = Initial uncertainty (process noise)
    @process_noise = Matrix[
      [0.3, 0], # We're not certian about height
      [0, 0.3]  # We're not certain about velocity
    ]

    # u = External motion (control signal)
    # There no control signal in our case, so it's just zeroes
    @control = Matrix[
      [0.0], # Height control signal, none
      [0.0]  # Velocity control signal, none
    ]

    # F = Next state function (process model)
    # This is where everything is goes nuts:
    # You need to come up with a matrix, that will represent a model
    # of changes between states. So if you apply this matrix to previous
    # state vector, you will get the next state prediction vector.
    @model = Matrix[
      [1.0, 1.0],
      [0, 1.0]
    ].freeze

    # H = Measurement function (measured params)
    @readings = Matrix[
      [1.0, # We can measure height
      0.0]  # But we can't measure velocity
    ].freeze

    # R = Measurement uncertainty (measurement noise)
    @sensor_noise = Matrix[
      [0.6] # We're sensing only height, and there some error associated with height
    ]

    # I = Identity matrix
    @identity = Matrix.identity(2).freeze
  end

  def update(value)
    ## Update stage

    # z = Current measurement
    measurement = Matrix[
      [value]
    ]

    # y = z - (H * x)
    error = measurement - (self.readings * self.state)

    # S = H * P * Ht + R
    system_uncertainty = self.readings * self.process_noise * self.readings.transpose + self.sensor_noise

    # K = P * Ht + S^-1
    kalman_gain = self.process_noise * self.readings.transpose * system_uncertainty.inverse

    # x = x + (K * y)
    self.state = self.state + (kalman_gain * error)

    # P = (I - (K * H)) * P
    self.process_noise = (self.identity - (kalman_gain * self.readings)) * self.process_noise

    predict!
  end

  def to_s
    {state: self.state, noise: self.process_noise}
  end
  alias_method :inspect, :to_s

  private
  def predict!
    ## Prediction stage

    # x = (F * x) + u
    self.state = (self.model * self.state) + self.control

    # P = F * P * Ft
    self.process_noise = self.model * self.process_noise * self.model.transpose
  end
end

kalman = Kalman.new
# This is a data from my untrasonic range sensor.
# Data represent distance from sensor to the well's water level. 
# Timestep is about 93ms.
# Estimated state is not good enough, since simple "height = last_height * velocity" is 
# not even close to real well model.
measurements = %w{245.24
245.03
245.24
245.72
245.59
245.24
245.72
245.45
245.24
245.72
245.24
245.10
245.03
245.72
245.79
245.72
245.59
245.24
245.59
245.17
245.59
245.72
245.59
245.24
245.10
245.59
245.10
245.59
245.72
245.66
245.59
245.79
245.59
245.03
245.24
245.59
245.72
245.24
245.59
245.24
245.31
245.59
245.24
245.52
245.24
245.10
245.59
245.24
244.90
245.72
245.66
245.52
245.72
245.10
245.59
245.24
245.72
244.97
245.03
245.59
245.72
245.24
245.52
245.24
245.10
245.24
245.10
245.17
245.24
245.66
245.59
245.24
245.17
245.24
245.59
245.24
245.59
245.03
245.10
245.52
245.79
245.72
245.24
245.72
245.24
245.59
245.24
245.66
245.24
245.03
245.24
245.17
245.24
245.17
245.79
245.72
245.24
245.03
245.24
245.10
245.72
245.24
245.59
245.24
245.52
245.24
245.59
245.24
245.72
245.24
245.03
245.59
245.10
245.24
245.72
245.24
245.79
245.24
245.10
245.59
245.24
245.72
245.24
245.10
245.59
245.24
245.17
245.79
245.45
245.24
245.79
245.03
245.24
245.59
245.24
245.59
245.24
245.59
245.24
245.52
245.59
245.03
245.10
245.66
245.24
245.10
245.24
245.59
245.17
245.79
245.10
245.03
245.24
245.72
245.24
245.72
245.59
245.24
245.59
245.10
245.31
245.24
245.17
245.24
245.72
245.24
245.10
245.59
245.24
245.72
245.59
245.24
245.79
245.24
245.10
245.24
245.38
245.17
245.24
245.59
245.24
245.17
245.10
245.24
245.17
245.24
245.59
245.38
245.24
245.31
245.24
245.72
245.24
245.72
245.52
245.24
245.72
245.59
245.24
245.38
245.72
245.59
245.24
245.72
245.24
245.52
245.24
245.79
245.24
245.66
245.24
245.72
245.24
245.66
245.59
245.24
245.59
245.24
245.79
245.59
245.24
245.72
245.59
245.79
245.72
245.59
245.38
245.59
245.66
245.24
245.17
245.72
245.24
245.45
245.59
245.24
245.79
245.72
245.59
245.24
245.72
245.79
245.59
245.52
245.79
245.17
245.24
245.59
245.24
245.10
245.24
245.59
245.79
245.24
245.59
245.24
245.59
245.79
245.24
245.59
245.24
245.79
245.59
245.24
245.72
245.24
245.59
245.24
245.17
245.24
245.59
245.72
245.10
245.59
245.03
245.24
245.31
245.59
245.52
245.24
245.79
245.52
245.66
245.72
245.79
245.24
245.59
245.72
245.24
245.59
245.17
245.24
245.59
245.72
245.66
245.24
245.72
245.24
245.72
245.66
245.59
245.24
245.59
245.66
245.59
245.24
245.72
245.59
245.79
245.24
245.66
245.24
245.59
245.79
245.17
245.66
245.24
245.72
245.38
245.59
245.24
245.72
245.24
245.59
245.72
245.31
245.52
245.24
245.59
245.24
245.38
245.59
245.72
245.24
245.59
245.24
245.10
245.24
245.59
245.66
245.24
245.72
245.24
245.17
245.31
245.24
245.72
245.31
245.24
245.59
245.24
245.59
245.24
245.45
245.24
245.79
245.59
245.24
245.59
245.24
245.66
245.59
245.24
245.59
245.24
245.79
245.59
245.52
245.79
245.24
245.59
245.72
245.24
245.10
245.59
245.72
245.24
245.31
245.24
245.79
245.72
245.24
245.38
245.24
245.59
245.72
245.24
245.72
245.79
245.66
245.72
245.79
245.59
245.72
245.24
245.10
245.24
245.79
245.59
245.31
245.59
245.24
245.72
245.24
245.52
245.79
245.31
245.59
245.72
245.45
245.66
245.24
245.59
245.72
245.79
245.24
245.72
245.79
245.59
245.24
245.72
245.24
245.38
245.24
245.72
245.45
245.38
245.59
245.24
245.72
245.79
245.66
245.72
245.59
245.79
245.59
245.24
245.45
245.31
245.79
245.24
245.72
245.79
245.59
245.52
245.59
245.24
245.52
245.38
245.31
245.79
245.24
245.72
245.38
245.72
245.79
245.24
245.45
245.24
245.72
245.24
245.79
245.59
245.24
245.72
245.10
245.59
245.24
245.59
245.24
245.59
245.72
245.79
245.59
245.24
245.79
245.10
245.59
245.24
245.79
245.59
245.24
245.45
245.52
245.24
245.17
245.59
245.72
245.24
245.72
245.31
245.24
245.38
245.24
245.59
245.24
245.10
245.59
245.24
245.72
245.66
245.24
245.45
245.59
245.24
245.72
245.79
245.52
245.72
245.24
245.66
245.24
245.79
245.38
245.72
245.59
245.24
245.72
245.17
245.24
245.79
245.72
245.24
245.66
245.52
245.72
245.79
245.24
245.72
245.79
245.24
245.59
245.72
245.24
245.45
245.24
245.52
245.24
245.79
245.72
245.66
245.59
245.72
245.59
245.24
245.72
245.66
245.24
245.72
245.24
245.52
245.24
245.72
245.24
245.72
245.79
245.59
245.24
245.10
245.59
245.24
245.31
245.24
245.72
245.24
245.72
245.59
245.24
245.45
245.24
245.72
245.24
245.79
245.45
245.66
245.72
245.24
245.79
245.52
245.59
245.24
245.38
245.72
245.31
245.72
245.38
245.72
245.24
245.59
245.24
245.79
245.45
245.79
}.map(&:to_f)

measurements.each do |value|
  kalman.update value
  puts "p: %.4f e: %.2f a: %.4f v: %.2f" % [kalman.process_noise.first, kalman.state.first, kalman.state[1,0], value]
  #puts kalman.to_s
end