demoKalman.tcl

# $Id$ #

#
#/** An example on using a kalman filter in tcl. \file demoKalman.tcl  \ingroup filter */
#

#
# LABARRERE "Le filtrage et ses application" Supaero
# Ch IX "Filtre de Kalman"
# IX.6 exemples
# p 236-237

# gnuplot commande
# plot 'demoKalman.dat' u 1:2 w l, '' u 1:4 w l, '' u 1:6 w l

package require kernel
package require jmath
package require filter


# the time step (s)
set dt "0.1"

set t 0
set x 0
set v 1

set z [jmath::new_vec 1]

set gamma 1
set r "0.1"

set F [jmath::new_mat 2 2]
jmath::setValue $F "((1, $dt), (0, 1))"

set filter_gamma $gamma
set filter_r $r

set Q [jmath::new_sym_mat 2]
jmath::setValue $Q \
"(([expr $filter_gamma*pow($dt,4)/4], [expr $filter_gamma*pow($dt,3)/2]), \
 ([expr $filter_gamma*pow($dt,3)/2], [expr $filter_gamma*pow($dt,2)]))"

set pm [filter::new_LinearPredictModel $F $Q]
$pm setTimeStep $dt

set H [jmath::new_mat 1 2]
jmath::setValue $H "((1, 0))"

set om [filter::new_LinearObserveModel $H]

set R [jmath::new_vec 1]
jmath::setValue $R "($filter_r)"

$om setUncorrelatedR $R

set Lambda_0 [jmath::new_sym_mat 2]
jmath::setValue $Lambda_0 \
"(($filter_r, 0), \
 (0,[expr 4*$filter_r/pow($dt,2)]))"

set X_0 [jmath::new_vec 2]
$X_0 clear

set myFilter [filter::new_KalmanFilter 2]
#$myFilter setCovUpdateType $::filter::BaseKalmanFilter_SIMPLE
#$myFilter setCovUpdateType $::filter::BaseKalmanFilter_STANDARD
$myFilter setCovUpdateType $::filter::BaseKalmanFilter_JOSEPH
$myFilter init $X_0 $Lambda_0

jmath::NormalDistribution myNormale 0 $r

set myLog [kernel::new_DataLogger "demoKalman.dat"]
$myLog addLoggable $myFilter

$myLog log

proc oneStep {} {
    global t dt x v z pm om myNormale myFilter myLog
    
    set x [expr "$x + $dt*$v"]
    set w [myNormale get]

    $z set 0 [expr "$x+$w"]
    
    $myFilter predict $pm
    $myFilter update $om $z

    $myLog log

}

proc step {{nSteps 50}} {
    # run nSteps of the filter
    for {set i 0} {$i < $nSteps} {incr i} {
        oneStep
    }
}