) 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)}]
KalmanFilterThe following image shows the filtered result (red line) of the measurement values (gray line):arjen - 2017-07-10 14:12:36This looks nice ... printed the page for further study :).
