Updated 2017-07-10 14:12:36 by arjen

JMeh 9 Jul 2017 - kalman

The Kalman algorithm can eliminate noise from a group of measurement values which belong together (see wikipedia article https://en.wikipedia.org/wiki/Kalman_filter) and is therefore a complicated mathematical algorithm full of matrix operations. But when we reduce the problem to only one variable (which contains the noise), this algorithm can be extremly simplified. I found a nice implementation (http://interactive-matter.eu/blog/2009/12/18/filtering-sensor-data-with-a-kalman-filter/) with only 4 lines of main code. There is also an implementation for Arduino (https://github.com/bachagas/Kalman).

Here is a little Tcl code which helps me freeing my measurements (weights from a scale during a steel remelting process) from noise:
#
# Kalman 1D - Filter
#
# https://de.wikipedia.org/wiki/Kalman-Filter
# https://github.com/bachagas/Kalman
# http://interactive-matter.eu/blog/2009/12/18/filtering-sensor-data-with-a-kalman-filter/
#

namespace eval ::kalman {
  variable state
}


proc ::kalman::init {id q r p initial_value} {
  variable state

  set state($id,q) $q                    ;# process noise covariance
  set state($id,r) $r                    ;# measurement noise covariance
  set state($id,p) $p                    ;# estimation error covariance
  set state($id,x) $initial_value        ;# filtered value 
  set state($id,k) 1.0                   ;# kalman gain

  return $id
}


proc ::kalman::update {id measurement} {
  variable state

  # prediction update
  set state($id,p) [expr {$state($id,p) + $state($id,q)}]

  # measurement update
  set state($id,k) [expr {$state($id,p) / ($state($id,p) + $state($id,r))}]
  set state($id,x) [expr {$state($id,x) + $state($id,k) * ($measurement - $state($id,x))}]
  set state($id,p) [expr {(1.0 - $state($id,k)) * $state($id,p)}]

  return $state($id,x)
}


proc ::kalman::get {id} {
  variable state

  return $state($id,x)
}


proc ::kalman::destroy {id} {
  variable state

  array unset state $id,*
}


package provide kalman 1.0

And here is a corresponding example with a set of my measurement values. The dialog contains three logarithmic sliders to setup the main parameters Q, R and P. You need the package RBC (or the original BLT for 8.3) to display the data (measurement values and filtered data).
proc LoadData {} {
  global ydata g

  set ydata {
    0 0 0 0 0 0 0 0 0 14 -1 22 9 8 8 10 0 4 14 7 9 14 2 11 13 20 7 5 13 10 14 9
    10 12 11 12 10 11 12 12 11 11 9 12 10 12 12 11 10 10 12 13 10 11 9 11 12 10
    10 11 10 10 9 9 13 11 10 11 10 10 11 11 10 10 10 11 10 9 10 9 12 9 10 10 10
    9 10 10 10 9 8 12 10 10 7 12 9 9 9 9 9 8 10 10 11 9 8 9 9 9 10 9 9 11 9 8
    10 10 9 10 9 9 10 9 9 9 10 8 9 10 9 10 11 10 10 8 9 11 9 10 9 8 9 9 9 10 10
    9 10 10 10 10 10 10 9 10 10 10 9 10 11 9 9 10 9 10 10 10 9 8 11 8 10 10 9 8
    13 12 10 9 10 9 11 9 9 9 8 8 9 11 12 11 10 11 10 11 10 10 9 9 7 10 9 9 10 10
    8 10 10 11 11 11 10 10 9 12 10 10 9 10 9 9 10 8 10 10 9 10 11 10 12 10 9 9
    9 10 8 10 10 11 12 10 9 9 9 9 8 10 10 12 12 10 11 9 9 9 9 11 10 8 9 9 10 11
    10 10 10 9 11 11 11 9 10 10 10 10 11 8 11 11 11 12 12 11 11 12 12 12 12 -1
    0 23 -3 -5 5 1 1 6 11 8 11 8 9 10 8 6 7 11 11 22 5 8 7 7 7 6 7 7 10 8 10 12
    11 10 5 10 9 7 10 11 9 10 7 9 8 10 6 11 10 7 10 6 12 9 8 6 12 8 8 7 11 8 10
    7 12 9 9 12 5 12 5 9 12 7 9 5 9 9 8 10 8 11 8 11 8 7 10 8 9 8 11 7 7 11 10
    10 6 7 9 10 9 6 11 10 8 7 10 12 6 10 8 7 9 10 10 8 7 11 12 10 10 9 7 9 10 8
    8 9 11 7 8 8 12 5 11 13 8 7 7 10 9 8 8 11 10 10 7 11 9 10 11 8 9 13 10 11 8
    9 7 10 9 10 8 10 8 11 10 9 10 7 9 10 8 11 11 7 10 6 12 11 8 7 8 11 10 11 10
    9 8 11 8 11 7 10 7 9 13 8 8 11 10 11 11 10 9 14 5 12 6 13 10 12 11 9 11 9 15
    7 8 7 10 10 10 9 11 11 10 9 9 10 10 10 9 11 11 10 9 8 10 9 9 9 7 11 10 10 10
    10 10 11 6 12 11 9 10 12 9 9 10 10 10 10 10 8 10 6 19 8 9 7 7 10 9 9 10 10 9
    9 13 15 8 7 8 11 13 7 10 10 8 9 9 7 8 11 16 13 18 16 13 14 16 10 10 6 3 0 0
    13 -7 3 2 10 11 21 10 11 10 11 10 9 9 9 12 11 10 8 9 6 9 10 11 9 8 8 8 9 10
    12 13 7 12 12 13 7 11 9 8 10 13 10 9 9 10 9 9 6 12 9 10 10 8 10 10 8 13 9 12
    9 11 9 11 10 8 9 10 8 8 12 8 8 7 11 10 24 8 6 6 7 9 6 7 8 18 7 11 8 9 9 7 7
    8 23 7 9 6 9 8 6 7 9 10 11 8 10 10 7 9 8 8 11 14 11 10 7 9 13 11 9 14 10 13
    23 0 -2 1 1 3 6 6 9 11 9 11 10 9 12 9 9 9 10 10 9 11 10 7 7 10 9 11 10 8 10
    9 6 9 7 10 8 10 9 11 10 9 11 8 8 9 10 8 8 10 10 8 8 11 8 8 8 8 11 12 6 10 6
    7 10 11 10 9 9 11 9 8 9 8 8 9 10 12 6 9 7 9 10 10 9 9 8 10 10 8 6 10 10 9 8
    9 9 10 9 6 9 9 8 8 9 6 11 13 5 7 11 7 10 11 4 10 7 8 9 10 8 9 9 12 10 7 8 8
    7 9 11 10 8 9 10 8 6 8 8 7 14 7 9 9 9 8 12 8 11 8 8 10 10 8 8 9 12 8 9 8 9 8
    11 10 9 9 7 10 10 11 5 9 11 10 10 10 8 10 11 7 11 10 8 9 8 10 9 11 11 8 10 9
    10 10 10 9 9 8 11 11 11 8 17 8 10 11 7 9 8 9 12 8 11 8 11 9 10 10 8 6 6 5 0
    3 2 2 3 5 4 0 3 4 3 3 3 1 1 10 0 0 1 -1 0 0 0 1 0 2 0 1 0 0 1 0 0 -1 1 1 -3
    -1 1 1 0 0 0 0 0 0 0 0 2 -1 0 1 -1 1 1 0 -1 0 1 -1 0 0 1 0 -1 -2 4 -1 0 0 0
    1 -1 0 0 1 0 0 0 1 0 -1
  }
  set cnt [llength $ydata]
  set xdata {}
  for {set i 0} {$i < $cnt} {incr i} {
    lappend xdata $i
  }
  $g element configure line1 -xdata $xdata -ydata $ydata
  $g element configure line2 -xdata $xdata 
}


proc KalmanFilter {} {
  global ydata g q r p

  set filter_data {}
  kalman::init USR $q $r $p 0.0
  foreach value $ydata {
    lappend filter_data [kalman::update USR $value]
  }
  $g element configure line2 -ydata $filter_data
}


proc StartKalmanFilter {varname value} {
  upvar #0 $varname var
  set var [format %.6f [expr {pow(10,$value)}]]
  after cancel KalmanFilter
  after idle KalmanFilter
}


proc Dialog {} {
  global g

  uplevel #0 {
    package require rbc
    # package require kalman
    source kalman.tcl
  }

  set g .g
  grid [rbc::graph $g -width 800 -height 600] -row 0 -column 0 -sticky nswe
  $g legend configure -hide yes
  $g grid on
  $g axis configure x -stepsize 60 -subdivisions 1
  $g element create line2 -symbol {} -color red -linewidth 3
  $g element create line1 -symbol {} -color gray50 -linewidth 1
  grid columnconfigure . 0 -weight 1
  grid rowconfigure . 0 -weight 1

  grid [ttk::labelframe .f -text "Kalman-Filter Parameter:"] -row 1 -column 0 -sticky we

  grid [ttk::label .f.l1 -text "q"] -row 0 -column 0 -sticky w -padx 1m -pady 1m
  grid [ttk::scale .f.s1 -orient horizontal -from -5 -to 2 -variable log_q -command [list StartKalmanFilter q]] -row 0 -column 1 -sticky we -padx 1m -pady 1m
  grid [ttk::entry .f.e1 -state readonly -justify right -textvar q -width 14] -row 0 -column 2 -sticky w -padx 1m -pady 1m
  grid [ttk::label .f.t1 -text "(process noise covariance)"] -row 0 -column 3 -sticky w -padx 1m -pady 1m

  grid [ttk::label .f.l2 -text "r"] -row 1 -column 0 -sticky w -padx 1m -pady 1m
  grid [ttk::scale .f.s2 -orient horizontal -from -3 -to 6 -variable log_r -command [list StartKalmanFilter r]] -row 1 -column 1 -sticky we -padx 1m -pady 1m
  grid [ttk::entry .f.e2 -state readonly -justify right -textvar r -width 14] -row 1 -column 2 -sticky w -padx 1m -pady 1m
  grid [ttk::label .f.t2 -text "(measurement noise covariance)"] -row 1 -column 3 -sticky w -padx 1m -pady 1m

  grid [ttk::label .f.l3 -text "p"] -row 2 -column 0 -sticky w -padx 1m -pady 1m
  grid [ttk::scale .f.s3 -orient horizontal -from -1 -to 3 -variable log_p -command [list StartKalmanFilter p]] -row 2 -column 1 -sticky we -padx 1m -pady 1m
  grid [ttk::entry .f.e3 -state readonly -justify right -textvar p -width 14] -row 2 -column 2 -sticky w -padx 1m -pady 1m
  grid [ttk::label .f.t3 -text "(estimation error covariance)"] -row 2 -column 3 -sticky w -padx 1m -pady 1m

  grid columnconfigure .f 1 -weight 1
}


Dialog
LoadData
set q 0.01
set r 10.0
set p 100.0
set log_q [expr {log10($q)}]
set log_r [expr {log10($r)}]
set log_p [expr {log10($p)}]
KalmanFilter

The following image shows the filtered result (red line) of the measurement values (gray line):


arjen - 2017-07-10 14:12:36

This looks nice ... printed the page for further study :).