High-pass filter

In one of my posts I wrote about low-pass IIR filter (of 1^st order). Here is the simple high-pass filter in form of IIR of 1^st order. See more info here.
In my application HMC demo I poll sensor (accelerometer) and filter Ax signal:

HPFilter hpf
hpf configure -datatype flt -datatypes {phys}
Saver saver1
saver1 listen -on
saver listen -on
listen phvals:hpf
listen hpf:saver1
serial open 4
sensor poll -loop

This will create 2 CSV files with native and with filtered signals (after saving we rename them). We can render signals with GLE tool:

begin graph
    title "Accelerations"
    xtitle "Time [sec]"
    ytitle "Ax [g]"
    axis grid
    ticks color grey40
    subticks on
    subticks lstyle 2
    set font "texcmr"

    d1 file "c:/tmp/saver.csv,0,5"
    let d2 = d1
    d1 line color red
    d2 deresolve 10 average line color red lwidth 0.1

    d3 file "c:/tmp/saver1.csv,0,5"
    let d4 = d3
    d3 line color blue
    d4 deresolve 10 average line color blue lwidth 0.1
end graph

begin key
    set fill yellow
    line color red text "Native"
    line color blue text "Filtered"
end key

and get this image:

 Blue line is signal after high-pass filtering: it shows effect of removing slow drift.
 Removing of slow drift is used in position estimation of sensor (from acceleration) in multicascading scheme:

HPF -> INTEGR -> HPF -> INTEGR -> HPF

Here is the C code (for SWIG) of this kind of filter:

double* filter(double *v) {
        int i;
        int vlen;

        vlen = (int)v[0];
        if (vlen != $self->n) {
            return (NULL);
        }

        for (i=1; i <= vlen; i++) {
            $self->v[i] = $self->a * ($self->v[i] + v[i] - $self->x[i]);
            $self->x[i] = v[i];
        }
        return ($self->v);
    }

$self is the pointer to CHPFilter struct:

typedef struct CHPFilter {
        int n; // length of v
        double a; // alpha
        double *v; // calculated values (keep prev. before filter)
        double *x; // prev. input
} CHPFilter;

Docstrings in Tcl (and IncrTcl)

Docstrings are very popular in Python, Lisp... This is the easy way to include documentation into implementation (reverse LP). But in Tcl are can be usable to generate help of commands for user. Here is the listing of one (it support procs and Itcl methods):

package provide mydoc 1.0

namespace eval mydoc {
    namespace export doc
}

set _NOHELPMSG "No help."

proc ::mydoc::_docstring {body} {
    set body [string trim $body]
    set docstring ""
    # without 1st '^' will match any FIRST docstring block even after
    # commands!
    if {[regexp {^#\s*([^\n]+\n)+(\n\n)} $body docstring]} {
        set docstring [regsub -all {\s*#\s?} $docstring \n]
        set docstring [string trim $docstring]
        return $docstring
    }
}

proc ::mydoc::doc args {
    # Help on command: procedure or class method. Call:
    #   doc some_object some_method
    #   doc some_class some_method
    #   doc some_proc


    global _NOHELPMSG
    set found ""
    switch -- [llength $args] {
        1 {
            # args: proc
            set name [lindex $args 0]
            set arguments [info args $name]
            set body [info body $name]
            set found [_docstring $body]
        }
        2 {
            # FIXME not optimal!
            # args: object|class method
            lassign $args cls_obj meth
            set objs [itcl::find objects]
            # cls_obj may be object OR class. What is it?
            if {-1 != [lsearch -regexp $objs :*$cls_obj]} {
                # this is the object
                set arguments [$cls_obj info args $meth]
                set body [$cls_obj info body $meth]
                set found [_docstring $body]
            } else {
                # this is the class
                set arguments [namespace eval ::$cls_obj info args $meth]
                set body [namespace eval ::$cls_obj info body $meth]
                set found [_docstring $body]
            }
        }
        default { error "wrong args: proc | object method | class method" }
    }
    if {$found eq ""} {
        return $_NOHELPMSG
    } else {
        return $found
    }
}

# txt is the string with \n, shifter is like '\t' or '\t\t'..
proc mydoc::_shift_strings {txt shifter} {
    if {$txt ne ""} {
        return "$shifter[regsub -all \n $txt \n$shifter]"
    }
}


# Generate only for documented with docstrings
proc mydoc::_genrst {fname} {
    set result {}
    # Collect help on objects and it's methods
    set clshelp {}
    foreach cls [itcl::find classes] {
        set her [namespace eval $cls "info heritage"]

        set varhelp {}
        foreach v [namespace eval $cls info variable] {
            catch {
                #lappend varhelp [namespace eval $cls info variable $v -protection public]
                if {[string first "::_" $v] == -1} {
                    switch -- [namespace eval $cls info variable $v -protection] {
                        public { set vprot "public" }
                        protected { set vprot "protected" }
                        private { set vprot "private" }
                    }
                    lappend varhelp "- $vprot $v"
                }
            }
        }

        set methelp {}
        foreach func [namespace eval $cls "info function"] {
            catch {
                set body [string trim [namespace eval $cls "info body $func"]]
                if {$body ne ""} {
                    set arguments [namespace eval $cls "info args $func"]
                    if {$arguments eq ""} { set arguments "no args." }
                    set docstring [_shift_strings [_docstring $body] \t]
                    if {$docstring ne ""} {
                        lappend methelp "*${func}*: **${arguments}**"
                        lappend methelp ""
                        lappend methelp "::"
                        lappend methelp ""
                        lappend methelp $docstring
                        lappend methelp ""
                        lappend methelp ""
                    }
                }
            }
        }
        if {$methelp ne ""} {
            # there are methods with docstrings!
            if {[llength $her] > 1} {
                # there are base classes
                set bases [lrange $her 1 end]
                set her "[lindex $her 0] (*extends ${bases}*)"
            }
            lappend clshelp "$her"
            lappend clshelp [string repeat "-" [string length $her]]
            lappend clshelp ""
            lappend clshelp "Variables"
            lappend clshelp "~~~~~~~~~"
            lappend clshelp ""
            if {$varhelp ne ""} {
                lappend clshelp [join $varhelp "\n"]
            } else {
                lappend clshelp "No variables."
            }
            lappend clshelp ""
            lappend clshelp "Methods"
            lappend clshelp "~~~~~~~"
            lappend clshelp ""
            lappend clshelp {*}$methelp
        }
    }
    # Collect procs help
    set prochelp {}
    foreach func [uplevel #0 info procs] {
        catch {
            set body [string trim [uplevel #0 info body $func]]
            if {$body ne ""} {
                set arguments [uplevel #0 info args $func]
                if {$arguments eq ""} { set arguments "no args." }
                set docstring [_shift_strings [_docstring $body] \t]
                if {$docstring ne ""} {
                    lappend prochelp "*${func}*: **${arguments}**"
                    lappend prochelp ""
                    lappend prochelp "::"
                    lappend prochelp ""
                    lappend prochelp $docstring
                    lappend prochelp ""
                    lappend prochelp ""
                }
            }
        }
    }

    if {$clshelp ne ""} {
        lappend result ""
        lappend result "Classes"
        lappend result "======="
        lappend result ""
        lappend result {*}$clshelp
    }
    if {$prochelp ne ""} {
        lappend result ""
        lappend result "Procedures"
        lappend result "=========="
        lappend result ""
        lappend result {*}$prochelp
    }


    set fid [open $fname w]
    puts -nonewline $fid [join $result "\n"]
    close $fid
}

This mechanism supports not only interactive help system for user (doc proc) but also help generating (to ReStructured Text format). Example of using (remember that two empty lines are used to end docstring section:

proc fun {args} {
  # Do something. Syntax:
  #  -one -- first action
  #  -two -- second action


  .. code ..
}

# OR

itcl::class C {
  method f {args} {
    # My method. Syntax:
    #  f one two -- do something
    #  f three four -- do something else


    .. code ..
  }
}

and then call:

doc fun
doc C f
C cobj
doc cobj f

_genrst procedure is used to generate documentation. It looks like this:

Classes

::HMC6343Protocol

Variables

• protected ::HMC6343Protocol::this

Methods

::HMC6343Protocol::get: cmd what

Returns protocol info (what) about command cmd. What is:
  -def - definition of command (Tcl commands list to be execute for request)
  -resplen - length of response in bytes
  -respfmt - format of response
  -all - as fields as list

::HMC6343Protocol::unpack: cmd buf

Returns unpacked (as list) values from buf for cmd

::HMC6343Protocol::commands: no args.

Returns known commands names

::HMC6343Protocol::constructor: defs

Creates object.
  - defs is the list of CMD RESPONSE_LEN RESPONSE_FMT { Serial commands }...
  - RESPONSE_FMT is like in binary command

It's easy to use it in Makefile for help generation:

RST2HTML = C:/Python32/python.exe C:/Python32/Scripts/rst2html.py
hlp:
 tclkit.exe gendoc.tcl
 $(RST2HTML) hlp.rst hlp.html

Use tclkit.exe if you have Tk code (instead of tclkitsh.exe). Python and docutils Python package are used to trnasform .rst into .html. gendoc.tcl looks like this:

# Generate hlp.rst file with references to all classes, its methods
# and procedures with docstrings (see lib/mydoc.tcl)
package require starkit
starkit::startup
starkit::autoextend [file join $starkit::topdir]
package require myutl
set packages [glob -directory lib -tails "*.tcl"]
foreach pkg $packages {
    set pkg [regsub {.tcl$} $pkg ""]
    if {$pkg ne "pkgIndex"} {
        package require $pkg
    }
}
mydoc::_genrst help.rst
exit 0

it imports all packages from lib directory and generates help.rst file for its' procs and methods. Makefile magic:

make hlp

:)

Listeners/providers architecture in Tcl (IncrTcl)

This is very popular model for Dataflow (and DSP) applications and no needing to introduce it. So, only listing:

package provide mylsn 1.0
package require Itcl

# ListenEvent
# =====================================================================
itcl::class ListenEvent {
    public variable name
    public variable src
    public variable data
    constructor {aname asrc {adata ""}} {
        set name aname
        set src asrc
        set data adata
    }
}

# DataListener 
# =====================================================================
itcl::class DataListener {
    # list of understandable providers' datatypes. Empty - all
    public variable datatypes ""

    protected variable _event {};  # ListenEvent

    # all listeners
    public common all {}

    constructor {} { lappend all $this }

    destructor {
        set i [lsearch -exact $all $this]
        if {$i >= 0} { set all [lreplace $all $i $i] }
    }

    # FSM state: LISTEN|SESSION|OFF. Means:
    #   LISTEN - ready for data from data provider
    #   SESSION - already reading some data (first data packet was received)
    #   OFF - reading of data is disabled
    protected variable _lstate LISTEN

    # values is list of values
    protected method ondata {values} {}

    # on run polling (listening session). columns is the list of columns names,
    # units is the list of unit names
    protected method onrun {columns units} {}

    # on stop listening session
    protected method onstop {} {}

    # on add to data provider
    protected method onadd {provider} {}

    # on delete from data provider
    protected method ondel {provider} {}

    method event {ev src args} {
        # generate event (call callback) for this listener.
        # ev is ListenEvent object, src is the source of event.
        # ev is one of run|stop|data|on|off|add|del:
        #   run - before first packet sent
        #   stop - after last packet sent
        #   data - packet is received
        #   on - enable listening
        #   off - disable listening
        #   add - connect with some data provider
        #   del - disconnect from some data provider


        set _event [ListenEvent #auto $ev $src $args]
        switch -- $_lstate {
            LISTEN {
                switch -- $ev {
                    run  { set _lstate SESSION
                           catch { $this onrun {*}$args } }
                    off  { set _lstate OFF }
                    add  { catch { $this onadd {*}$args } }
                    del  { catch { $this ondel {*}$args } }
                }
            }

            SESSION {
                switch -- $ev {
                    stop { set _lstate LISTEN
                           catch { $this onstop } }
                    data { catch { $this ondata {*}$args } }
                }
            }

            OFF {
                switch -- $ev {
                    on   { set _lstate LISTEN }
                    add  { catch { $this onadd {*}$args } }
                    del  { catch { $this ondel {*}$args } }
                }
            }
        }
    }

    # listen -on|-off
    method listen {what} {
        # listen -on -- turn-on listening
        # listen -off -- turn-off listening


        # event without src ("" - user call is event source)
        switch -- $what {
            -on     { $this event on "" }
            -off    { $this event off "" }
            default { error  "listen can be -on or -off only" }
        }
        return
    }

    method listened {} {
        # Is listening now?


        return [expr {$_lstate ne "OFF"}]
    }

    # join columns and units with delimiter into new list
    method join_columns_units {columns units {delim ","}} {
        set res {}
        foreach c $columns u $units {
            lappend res "$c$delim$u"
        }
        return $res
    }
}

# DataProvider
# =====================================================================
itcl::class DataProvider {
    # static list of all providers
    public common all {}
    public variable datatype ""

    protected variable _listeners

    constructor {} { lappend all $this }

    destructor {
        set i [lsearch -exact $all $this]
        if {$i >= 0} { set all [lreplace $all $i $i] }
    }

    # returns list (pair) of columns (list) and units - if they are
    # fixed all the time (for any session)
    method fixed_columns_units {} {}

    # normalize name, need bcz user can use quilified name as
    # ::a, not a.
    # FIXME namespace which -variable does not work in Itcl, so
    # i cut all :, but is possible to add/del only listeners on
    # top-level namespace
    protected method _normname {name} {
        return [regsub -all ":" $name ""]
    }

    method get_listeners {} {
        # Returns names of all listeners


        return [array names _listeners]
    }

    method add_listener {listener} {
        # Add some listener


        set lsndts [$listener cget -datatypes]; # datatypes expected by listener
        if {[llength $lsndts] && !($datatype in $lsndts)} {
            # if listener datatypes not empty (expect some) and my datatype
            # is not in it's datatypes, so I can't add this listener
            error "Listener $listener does not understand $this provider"
        }

        set name [_normname $listener]
        if {[itcl::is object -class DataListener $listener]} {
            if {[array get _listeners $name] ne ""} {
                error "Listener $name already exists"
            }
            set _listeners($name) $listener
            $listener event add $this $this
        } else {
            error "listener should be DataListener object"
        }
    }

    method del_listener {listener {stop 1}} {
        # Deletes listener, sends before stop event if needed


        set name [_normname $listener]
        set listener [lindex [array get _listeners $name] 1]
        if {$listener ne ""} {
            if {$stop} { $listener event stop $this }
            array unset _listeners $name
            $listener event del $this $this
        }
        return $listener
    }

    # XXX not effective
    method del_all_listeners {{stop 1}} {
        # Deletes all listeners, send stop event before, if needed


        foreach name [array names _listeners] {
            del_listener $name $stop
        }
    }

    method notify_all {ev args} {
        # Notify all listeners with event ev and some args


        foreach {name listener} [array get _listeners] {
            $listener event $ev $this {*}$args
        }
    }

    method number {} {
        # Returns number of listeners


        return [array size _listeners]
    }

}

# ProxyListener - general object for process events without creating
# a special class
# =====================================================================
itcl::class ProxyListener {
    inherit DataListener DataProvider

    public variable onrunproc ""
    public variable onstopproc ""
    public variable ondataproc ""
    public variable onaddproc ""
    public variable ondelproc ""

    protected method onrun {columns units} {
        if {$onrunproc eq ""} {
            notify_all run $columns $units
        } else {
            $onrunproc $columns $units
        }
    }

    protected method onstop {} {
        if {$onstopproc eq ""} {
            notify_all stop
        } else {
            $onstopproc
        }
    }

    protected method ondata {values} {
        if {$ondataproc eq ""} {
            notify_all data $values
        } else {
            $ondataproc $values
        }
    }

    protected method onadd {provider} {
        if {$onaddproc eq ""} {
            notify_all add $provider
        } else {
            $onaddproc $provider
        }
    }

    protected method ondel {provider} {
        if {$ondelproc eq ""} {
            notify_all del $provider
        } else {
            $ondelproc $provider
        }
    }
}

# DebugListener - repeater
# =====================================================================
itcl::class DebugListener {
    inherit ProxyListener

    public variable fixed_cu ""

    constructor {} {
        $this configure -datatype phys -datatypes {v s flt raw phys} \
            -onrunproc [itcl::code $this onrunproc] \
            -onstopproc [itcl::code $this onstopproc] \
            -ondataproc [itcl::code $this ondataproc] \
    }

    method fixed_columns_units {} {
        return $fixed_cu
    }

    method onrunproc {columns units} {
        notify_all run $columns $units
        puts "*DEBUG_${this}* onrun: $columns, $units"
    }

    method onstopproc {} {
        notify_all stop
        puts "*DEBUG_${this}* onstop"
    }

    method ondataproc {values} {
        notify_all data $values
        puts "*DEBUG_${this}* ondata: $values"
    }
}

# procedures
# =====================================================================

proc debug_listen {d between0 between1} {
    #DebugListener $this.#auto
    $d configure -fixed_cu [$between0 fixed_columns_units]
    listen $between0:$d
    listen $d:$between1
}

proc listen args {
    # Set who listen who:
    #   listen provider...: listener...
    # or
    #   listen prov...: all
    # or
    #   listen -- return list of lists {provider {listeners}}
    # or
    #   listen -txt -- return formatted string (for user)
    # or
    #   listen -p -- returns formatted string with providers and it's datatypes
    # or
    #   listen -l -- returns formatted string with listeners and it's datatypes


    if {[lsearch -exact $args "-txt"] != -1} {
        # if there is "-txt" option, return formatted string
        set res {}
        foreach prov $DataProvider::all {
            set nprov [regsub -all ":" $prov ""]
            lappend res "$nprov: [join [$prov get_listeners] ,\ ]"
        }
        return [join $res "\n"]

    } elseif {[lsearch -exact $args "-p"] != -1} {
        set res {}
        foreach prov $DataProvider::all {
            set nprov [regsub -all ":" $prov ""]
            set dt [$prov cget -datatype]
            lappend res "$nprov: provides '$dt'"
        }
        return [join $res "\n"]

    } elseif {[lsearch -exact $args "-l"] != -1} {
        set res {}
        foreach lsn $DataListener::all {
            set nlsn [regsub -all ":" $lsn ""]
            set dt [join [$lsn cget -datatypes] "','"]
            lappend res "$nlsn: listens '$dt'"
        }
        return [join $res "\n"]

    } elseif {$args eq ""} {
        # if no args, returns listening table (all links)
        set res {}
        foreach prov $DataProvider::all {
            lappend res [list $prov [$prov get_listeners]]
        }
        return $res

    } else {
        # normalize args (make ':' like arg, not part of another arg)
        lassign [split [join $args " "] :] providers listeners

        # if there is 'all' in listeners then it'll be all of them :)
        if {[lsearch -exact $listeners all] != -1} {
            set listeners $DataListener::all
        }

        # delete each listener from ALL known providers then attach
        # to selected
        foreach lsn $listeners {
            foreach prov $DataProvider::all { $prov del_listener $lsn 1 }
            foreach prov $providers { $prov add_listener $lsn }
        }
    }
}

There are classes:

• ListenEvent
• DataListener (should be inherited)
• DataProvider (should be inherited)
• ProxyListener
• DebugListener

First 3 are clear. ProxyListener avoids creation of class - options-callbacks are used instead. Also, it's like bridge: listens and provides. DebugListener is proxy too, but for debug purpose (transparent bridge with console logging). Here is a scheme:

Procedure listen is used to link providers and listeners or to obtain all of providers/listeners in system, or to obtain current link scheme (with native or pretty text representation). debug_listen procedure is used to insert DebugListener between two nodes.

Real time (stream) integration with Simpson's rule

Turn on MathML plugin

About this method of numerical integration you can read here. To do it in streaming manner we should do little formal mathematics:

Composite Simpson's rule:

$\underset{a}{\overset{b}{\int }}f\left(x\right)dx\approx \frac{h}{3}\sum _{k=1,2}^{N-1}\left(f\left(x_{k-1}\right)+4f\left(x_k\right)+f\left(x_{k+1}\right)\right)$

where k=1,2 means from 1 step by 2, N is even intervals number.

The sum will be: $\frac{h}{3}\sum _{k=1,2}^{N-1}{S}_k$

Simpsone's method requires 3 points least so we can integrate not every point (sample) but through one, so neighboring samples are I_N-2 and I_N. Different between them is:

$I_N-I_{N-2}=\frac{h}{3}\left(\sum _{k=1,2}^{N-1}{S}_k-\sum _{k=1,2}^{N-3}{S}_k\right)=\frac{h}{3}\left(S_{N-1}+\sum _{k=1,2}^{N-3}{S}_k-\sum _{k=1,2}^{N-3}{S}_k\right)$

no S_N-2 member bcz of step k=2.

So difference is: $\frac{h}{3}{S}_{N-1}$

For example, $I_4-I_2=\frac{h}{3}\left(y_2+4y_3+y_4\right)$, where I₄ is 4^th interval but 5^th sample.

And resulting formula of I_i=

$0,i\le 1$
$\frac{h}{3}\left(y_{i-2}+4y_{i-1}+y_i\right)+I_{i-1},imod2=0$
$I_{i-1},imod2\ne 0$

where i is a number of input sample. We can not get values of I on each sample, but through one (skipping odds and 0-th, 1-th). On odds samples, value will repeat previous one.

Example in SWIG:
some.h file:

// Simpsone's rule integrator
typedef struct CSIntr {
        double I; // last value of integral
        double y_1; // y[-1]
        double y_2; // y[-2]
        double h; // step of subinterval
        double h_d; // step of subinterval/3
        long long i; // iteration
} CSIntr;

some.i file:

%include "some.h"
%extend CSIntr {
    // FIXME how is more right to do this?
    void reset() {
        $self->I = 0.;
        $self->y_1 = $self->y_2 = 0.;
        $self->i = 0;
    }

    void setup(double h) {
        $self->h = h;
        $self->h_d = h/3.;
    }

    CSIntr(double h) {
        CSIntr *obj;

        if (NULL == (obj=(CSIntr*) malloc(sizeof (CSIntr))))
            return (NULL);
        CSIntr_reset(obj);
        CSIntr_setup(obj, h);
        return (obj);
    }

    ~CSIntr() {
        if ($self) free($self);
    }

    double calculate(double y) {
        if ($self->i <= 1)
            $self->I = 0;
        else if (0 == $self->i % 2)
            $self->I += $self->h_d * ($self->y_2 + 4. * $self->y_1 + y);

        $self->y_2 = $self->y_1;
        $self->y_1 = y;
        $self->i++;
        return ($self->I);
    }
};

So, if we use Tcl, then we can build and test our class:
Makefile's fragment:

some_wrap.c: some.i some.h some.c
 swig -tcl -pkgversion 1.0 some.i

some.dll: some_wrap.c
 gcc -Ic:/tcl/include -I./ -DUSE_TCL_STUBS -c some_wrap.c -o some_wrap.o
 gcc -shared -o some.dll some/some_wrap.o -L c:/tcl/lib -ltclstub85 -lm

test on linear function with y_i = {1, 2, 3, 4, 5}:

load some
% CSIntr cs 1
_f85f7601_p_CSIntr
% cs calculate 1
0.0
% cs calculate 2
0.0
% cs calculate 3
4.0
% cs calculate 4
4.0
% cs calculate 5
12.0

I₄ is really 12.0