Menu
▾
▴
[Octave-cvsupdate] SF.net SVN: octave:[10108] trunk/octave-forge/extra/tsa/src
|
From: <sch...@us...> - 2012-03-30 08:44:26
|
Revision: 10108
http://octave.svn.sourceforge.net/octave/?rev=10108&view=rev
Author: schloegl
Date: 2012-03-30 08:44:19 +0000 (Fri, 30 Mar 2012)
Log Message:
-----------
initial import of Martin Billinger's code
Added Paths:
-----------
trunk/octave-forge/extra/tsa/src/kalman_maar.cpp
trunk/octave-forge/extra/tsa/src/kalman_maar.h
Added: trunk/octave-forge/extra/tsa/src/kalman_maar.cpp
===================================================================
--- trunk/octave-forge/extra/tsa/src/kalman_maar.cpp (rev 0)
+++ trunk/octave-forge/extra/tsa/src/kalman_maar.cpp 2012-03-30 08:44:19 UTC (rev 10108)
@@ -0,0 +1,219 @@
+// Copyright by Martin Billinger
+
+// This program is free software; you can redistribute it and/or modify it under
+// the terms of the GNU General Public License as published by the Free Software
+// Foundation; either version 2 of the License, or (at your option) any later
+// version.
+//
+// This program is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License along with
+// this program; if not, write to the Free Software Foundation, Inc., 59 Temple
+// Place - Suite 330, Boston, MA 02111-1307, USA.
+
+#include "mex.h"
+#include "memory.h"
+
+#ifndef WIN32
+#include "blas.h"
+#include "lapack.h"
+#endif
+
+#include "kalman_maar.h"
+
+// input to the matlab function. for convenience elements
+// can be accessed by name as well as array element.
+union mexin
+{
+ struct {
+ const mxArray *y, *p, *UC, *Kalman, *mode;
+ };
+ const mxArray *list[5];
+};
+
+// output of the matlab function. for convenience elements
+// can be accessed by name as well as array element.
+union mexout
+{
+ struct {
+ mxArray *x, *e, *Kalman;
+ };
+ mxArray *list[3];
+};
+
+struct mxKalman
+{
+ mxArray *G, *H, *Kp, *Q1, *Q2, *errCov, *x, *yr;
+ struct Kalman pointers;
+};
+
+// true when persistent variables are initialized
+static bool initialized = false;
+
+// holds persistent variables used by the MVAAR algorithm
+static Kalman_Helper *kh = NULL;
+
+// parameters
+int M = 0, L = 0, p = 0;
+
+void initialize( ptrdiff_t M, ptrdiff_t p )
+{
+ mexPrintf( "Initializing Persistent variables.\n" );
+ kh = new Kalman_Helper( M, p );
+ initialized = true;
+}
+
+void cleanup( )
+{
+ mexPrintf( "Destroying Persistent variables.\n" );
+ delete kh;
+ kh = NULL;
+ initialized = false;
+}
+
+void mexFunction(
+ int nlhs, mxArray *plhs[], // output
+ int nrhs, const mxArray *prhs[] ) // input
+{
+ union mexin in;
+ union mexout out;
+ struct mxKalman inKalman, outKalman;
+
+ // some pointers for various uses
+ double *tmp1, *tmp2, *tmp3;
+
+ int n;
+ double UC;
+ double *y, *err;
+ double mode;
+
+ if( nrhs < 3 )
+ mexErrMsgTxt( "Not enough input arguments" );
+
+ if( nrhs > 5 )
+ mexErrMsgTxt( "Too many input arguments" );
+
+ for( n=0; n<nrhs; n++ )
+ in.list[n] = prhs[n];
+
+ if( nrhs < 5 )
+ mode = 7;
+ else
+ mode = *mxGetPr( in.mode );
+
+ y = mxGetPr( in.y );
+
+ // if any parameters is changed the persistent variables have to be
+ // initialized anew to accomodate for eventually changed memory requirements
+ if( ( p!=(int)mxGetPr(in.p)[0] ) || ( M!=mxGetN(in.y) ) )
+ if( initialized )
+ cleanup( );
+
+ if( !initialized )
+ {
+ p = (int)mxGetPr(in.p)[0];
+ M = mxGetN( in.y );
+ L = M*M*p;
+ initialize( M, p );
+ mexAtExit( cleanup );
+ }
+
+ UC = mxGetPr( in.UC )[0];
+
+ // create output variables
+ { // (in C variables can only be declared et the beginning of a block)
+ const char *fieldnames[8] = { "G", "H", "Kp", "Q1", "Q2", "errCov", "x", "yr" };
+ plhs[0] = mxCreateDoubleMatrix( 1, L, mxREAL ); // x
+ plhs[1] = mxCreateDoubleMatrix( 1, M, mxREAL ); // err
+ plhs[2] = mxCreateStructMatrix( 1, 1, 8, fieldnames ); // Kalman
+ }
+
+ for( n=0; n<3; n++ )
+ out.list[n] = plhs[n];
+
+ err = mxGetPr( out.e );
+
+ if( nrhs == 3 ) // no filter-state argument given. initialize filter
+ {
+ printf( "Initialising Kalman Filter\n" );
+ outKalman.G = mxCreateDoubleMatrix( L, M, mxREAL );
+ outKalman.H = mxCreateDoubleMatrix( M, L, mxREAL );
+ outKalman.Kp = eye( L, 1.0 );
+ outKalman.Q1 = eye( L, UC );
+ outKalman.Q2 = eye( M, 1.0 );
+ outKalman.errCov = eye( M, 1.0 );
+ outKalman.x = mxCreateDoubleMatrix(L,1,mxREAL);
+ outKalman.yr = mxCreateDoubleMatrix(1,M*p,mxREAL);
+
+ // Kalman.yr = [y(:)' zeros(1,M*(p-1))];
+ // err = y;
+ /*tmp1 = mxGetPr( in.y );
+ tmp2 = mxGetPr( outKalman.yr );
+ tmp3 = mxGetPr( out.e );
+ for( n=0; n<M; n++ )
+ {
+ *(tmp2++) = *(tmp1);
+ *(tmp3++) = *(tmp1++);
+ }*/
+ }
+ else
+ {
+ // extract filter-state from input data
+ inKalman.G = mxGetField( in.Kalman, 0, "G" );
+ inKalman.H = mxGetField( in.Kalman, 0, "H" );
+ inKalman.Kp = mxGetField( in.Kalman, 0, "Kp" );
+ inKalman.Q1 = mxGetField( in.Kalman, 0, "Q1" );
+ inKalman.Q2 = mxGetField( in.Kalman, 0, "Q2" );
+ inKalman.errCov = mxGetField( in.Kalman, 0, "errCov" );
+ inKalman.x = mxGetField( in.Kalman, 0, "x" );
+ inKalman.yr = mxGetField( in.Kalman, 0, "yr" );
+
+ // set pointers
+ inKalman.pointers.G = mxGetPr( inKalman.G );
+ inKalman.pointers.H = mxGetPr( inKalman.H );
+ inKalman.pointers.Kp = mxGetPr( inKalman.Kp );
+ inKalman.pointers.Q1 = mxGetPr( inKalman.Q1 );
+ inKalman.pointers.Q2 = mxGetPr( inKalman.Q2 );
+ inKalman.pointers.errCov = mxGetPr( inKalman.errCov );
+ inKalman.pointers.x = mxGetPr( inKalman.x );
+ inKalman.pointers.yr = mxGetPr( inKalman.yr );
+
+ // initialize filter-state output
+ outKalman.G = mxCreateDoubleMatrix( L, M, mxREAL );
+ outKalman.H = mxCreateDoubleMatrix( M, L, mxREAL );
+ outKalman.Kp = mxCreateDoubleMatrix(L, L, mxREAL );
+ outKalman.Q1 = mxCreateDoubleMatrix(L, L, mxREAL );
+ outKalman.Q2 = mxCreateDoubleMatrix(M, M, mxREAL );
+ outKalman.errCov = mxCreateDoubleMatrix(M, M, mxREAL );
+ outKalman.x = mxCreateDoubleMatrix( L, 1, mxREAL );
+ outKalman.yr = mxCreateDoubleMatrix(1,M*p,mxREAL );
+
+ // set pointers
+ outKalman.pointers.G = mxGetPr( outKalman.G );
+ outKalman.pointers.H = mxGetPr( outKalman.H );
+ outKalman.pointers.Kp = mxGetPr( outKalman.Kp );
+ outKalman.pointers.Q1 = mxGetPr( outKalman.Q1 );
+ outKalman.pointers.Q2 = mxGetPr( outKalman.Q2 );
+ outKalman.pointers.errCov = mxGetPr( outKalman.errCov );
+ outKalman.pointers.x = mxGetPr( outKalman.x );
+ outKalman.pointers.yr = mxGetPr( outKalman.yr );
+
+ // filtering step
+ kalman_maar( err, y, UC, M, L, p,
+ &inKalman.pointers, &outKalman.pointers, kh, mode );
+ }
+
+ // x = Kalman.x'
+ memcpy( mxGetPr( out.x ), mxGetPr( outKalman.x ), L*sizeof(double) );
+
+ mxSetField( out.Kalman, 0, "G", outKalman.G );
+ mxSetField( out.Kalman, 0, "H", outKalman.H );
+ mxSetField( out.Kalman, 0, "Kp", outKalman.Kp );
+ mxSetField( out.Kalman, 0, "Q1", outKalman.Q1 );
+ mxSetField( out.Kalman, 0, "Q2", outKalman.Q2 );
+ mxSetField( out.Kalman, 0, "errCov", outKalman.errCov );
+ mxSetField( out.Kalman, 0, "x", outKalman.x );
+ mxSetField( out.Kalman, 0, "yr", outKalman.yr );
+}
Added: trunk/octave-forge/extra/tsa/src/kalman_maar.h
===================================================================
--- trunk/octave-forge/extra/tsa/src/kalman_maar.h (rev 0)
+++ trunk/octave-forge/extra/tsa/src/kalman_maar.h 2012-03-30 08:44:19 UTC (rev 10108)
@@ -0,0 +1,403 @@
+// Copyright by Martin Billinger
+
+// This program is free software; you can redistribute it and/or modify it under
+// the terms of the GNU General Public License as published by the Free Software
+// Foundation; either version 2 of the License, or (at your option) any later
+// version.
+//
+// This program is distributed in the hope that it will be useful, but WITHOUT
+// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License along with
+// this program; if not, write to the Free Software Foundation, Inc., 59 Temple
+// Place - Suite 330, Boston, MA 02111-1307, USA.
+
+// struct Kalman
+// holds all information to describe a
+// kalman filter and it's current state
+struct Kalman
+{
+ // C-style pointers to matrices and vectors that
+ // describe a kalman filter and it's current state
+ double *G, *H, *Kp, *Q1, *Q2, *errCov, *x, *yr;
+};
+
+// struct Kalman_Helper
+// holds temporary variables that are
+// used by the algorithm
+struct Kalman_Helper
+{
+ Kalman_Helper( ptrdiff_t M, ptrdiff_t p )
+ {
+ ptrdiff_t L = M*M*p;
+ ye = new double[M];
+ ipiv = new ptrdiff_t[M];
+ tmp = new double[M*M];
+ KpH = new double[L*M];
+ HKp = new double[M*L];
+ HKpH = new double[M*M];
+ K = new double[L*L];
+ }
+
+ ~Kalman_Helper( )
+ {
+ delete[] ye;
+ delete[] ipiv;
+ delete[] tmp;
+ delete[] KpH;
+ delete[] HKp;
+ delete[] HKpH;
+ delete[] K;
+ }
+
+ double *ye;
+ ptrdiff_t *ipiv;
+ double *tmp;
+ double *KpH;
+ double *HKp;
+ double *HKpH;
+ double *K;
+};
+
+// BLAS' and LAPACK's fortran functions need
+// all their arguments to be passed by reference.
+// Therefore we need constants that can be referenced.
+double one = 1.0, zero = 0.0;
+double minusone = -1.0;
+ptrdiff_t inc = 1;
+char *chn="N"; char *cht="T";
+
+// Helper functions to create identity matrices
+void deye( int N, double *i, double value );
+mxArray *eye( int N, double value );
+
+
+// Implementation of the mvaar algorithm
+void kalman_maar( double *err, const double *y,
+ //double UC, int M, int L, int p,
+ double UC, ptrdiff_t M, ptrdiff_t L, ptrdiff_t p,
+ struct Kalman *inKalman, struct Kalman *outKalman,
+ struct Kalman_Helper *kh, int mode )
+{
+ double traceK;
+ double *tmp1, *tmp2, *tmp3;
+ ptrdiff_t n,i,j, info, index;
+
+ // % update Measurement Matrix
+ // Kalman.H = kron( eye(M), Kalman.yr );
+ index = 0;
+ for( j=0; j<M; j++ ) {
+ for( i=0; i<M*p; i++ ) {
+ outKalman->H[index] = inKalman->yr[i];
+ index += M;
+ }
+ index += 1;
+ }
+
+ // % calculate prediction error
+ // ye = (Kalman.H*Kalman.x)';
+ // err = y - ye
+ index = 0;
+ for( j=0; j<M; j++ ) {
+ kh->ye[j] = 0;
+ for( i=0; i<M*p; i++ )
+ kh->ye[j] += inKalman->yr[i] * inKalman->x[index++];
+ err[j] = y[j] - kh->ye[j];
+ }
+
+
+ // % Adaptive Error covariance
+ // Kalman.errCov = (1-UC)*Kalman.errCov + UC*(err'*err);
+ index = 0;
+ for( i=0; i<M; i++ )
+ for( j=0; j<M; j++ ) {
+ outKalman->errCov[index] = (1-UC)*inKalman->errCov[index] + UC * err[i] * err[j];
+ index++;
+ }
+
+ if( mode == 0 )
+ {
+ // Kalman.Q2 = Kalman.Q2; % No update
+ memcpy( outKalman->Q2, inKalman->Q2, sizeof(double)*M*M );
+ }
+ else
+ {
+ // % update Q2 using the prediction error
+ // % of the previous step (corresponds to BioSigs's eMode==4)
+ // Kalman.Q2 = Kalman.errCov;
+ memcpy( outKalman->Q2, inKalman->errCov, sizeof(double)*M*M );
+ }
+
+ // KpH = Kalman.Kp * Kalman.H';
+ dgemm( chn, cht, &L, &M, &L, &one, inKalman->Kp, &L,
+ outKalman->H, &M, &zero, kh->KpH, &L );
+
+ // HKp = Kalman.H * Kalman.Kp;
+ dgemm( chn, chn, &M, &L, &L, &one, outKalman->H, &M,
+ inKalman->Kp, &L, &zero, kh->HKp, &M );
+
+ // HKpH = Kalman.H * KpH
+ dgemm( chn, chn, &M, &M, &L, &one, outKalman->H, &M,
+ kh->KpH, &L, &zero, kh->HKpH, &M );
+
+
+ // % Kalman Gain
+ // Kalman.G = KpH * inv( HKpH + Kalman.Q2 );
+ tmp1 = kh->HKpH;
+ tmp2 = outKalman->Q2;
+ for( n=0; n<M*M; n++ )
+ *(tmp1++) += *(tmp2++);
+ memset( kh->tmp, 0, M*M*sizeof( double ) );
+ deye( M, kh->tmp, 1.0 );
+ dgesv( &M, &M, kh->HKpH, &M, kh->ipiv,
+ kh->tmp, &M, &info );
+ dgemm( chn, chn, &L, &M, &M, &one, kh->KpH, &L,
+ kh->tmp, &M, &zero, outKalman->G, &L );
+
+ // % calculation of the a-posteriori state
+ // % error covariance matrix
+ // K = Kalman.Kp-Kalman.G*HKp;
+ tmp1 = kh->K;
+ tmp2 = inKalman->Kp;
+ for( n=0; n<L*L; n++ )
+ *(tmp1++) = *(tmp2++);
+ dgemm( chn, chn, &L, &L, &M, &minusone, outKalman->G,
+ &L, kh->HKp, &M, &one, kh->K, &L );
+
+
+ // % updating Q1
+ if( mode == 0 )
+ {
+ // Kalman.Q1 = Kalman.Q1; % no update
+ memcpy( outKalman->Q1, inKalman->Q1, sizeof(double)*L*L );
+ }
+ else
+ {
+ // % corresponds to aMode==17
+ // K = 0.5 * (K+K');
+ // Kalman.Q1 = UC * Kalman.Kp;
+
+ for( j=0; j<L; j++ )
+ for( i=0; i<=j; i++ )
+ {
+ size_t ij = i * L + j;
+ size_t ji = j * L + i;
+ kh->K[ij] = 0.5 * ( kh->K[ij] + kh->K[ji] );
+ kh->K[ji] = kh->K[ij];
+ }
+
+ for( n=0; n<L*L; n++ )
+ outKalman->Q1[n] = UC * inKalman->Kp[n];
+ }
+
+ // % a-priori state error covariance matrix
+ // % for the next time step
+ // Kalman.Kp=K+Kalman.Q1;
+ tmp1 = outKalman->Kp;
+ tmp2 = kh->K;
+ tmp3 = outKalman->Q1;
+ for( n=0; n<L*L; n++ )
+ *(tmp1++) = *(tmp2++) + *(tmp3++);
+
+ // % current estimation of state x
+ // Kalman.x = Kalman.x + Kalman.G*err';
+ tmp1 = outKalman->x;
+ tmp2 = inKalman->x;
+ for( n=0; n<L; n++ )
+ *(tmp1++) = *(tmp2++);
+ dgemv( chn, &L, &M, &one, outKalman->G, &L, err,
+ &inc, &one, outKalman->x, &inc );
+
+ // % add new observation, drop oldest
+ // Kalman.yr = [y(:)' Kalman.yr(1:M*(p-1))];
+ tmp1 = outKalman->yr;
+ const double *tmpc = y;
+ for( n=0; n<M; n++ )
+ *(tmp1++) = *(tmpc++);
+ tmp2 = inKalman->yr;
+ for( n=0; n<M*(p-1); n++ )
+ *(tmp1++) = *(tmp2++);
+}
+
+
+// Implementation of the mvaar algorithm, that uses only one state variable
+void kalman_maar( double *err, const double *y,
+ //double UC, int M, int L, int p,
+ double UC, ptrdiff_t M, ptrdiff_t L, ptrdiff_t p,
+ struct Kalman *state,
+ struct Kalman_Helper *kh, int mode )
+{
+ double traceK;
+ double *tmp1, *tmp2, *tmp3;
+ ptrdiff_t n,i,j, info, index;
+
+ // % update Measurement Matrix
+ // Kalman.H = kron( eye(M), Kalman.yr );
+ // this loop should be faster than calculating the kronecker tensor product.
+ index = 0;
+ for( j=0; j<M; j++ ) {
+ for( i=0; i<M*p; i++ ) {
+ state->H[index] = state->yr[i];
+ index += M;
+ }
+ index += 1;
+ }
+ // % calculate prediction error
+ // ye = (Kalman.H*Kalman.x)';
+ // err = y - ye
+ index = 0;
+ for( j=0; j<M; j++ ) {
+ kh->ye[j] = 0;
+ for( i=0; i<M*p; i++ )
+ kh->ye[j] += state->yr[i] * state->x[index++];
+ err[j] = y[j] - kh->ye[j];
+ }
+
+ // % update of Q2 using the prediction error
+ // of the previous step
+ // Kalman.Q2 = (1-UC)*Kalman.Q2 + UC*err'*err;
+ index = 0;
+ for( i=0; i<M; i++ )
+ for( j=0; j<M; j++ ) {
+ state->Q2[index] = (1-UC)*state->Q2[index] + UC * err[i] * err[j];
+ index++;
+ }
+
+ // KpH = Kalman.Kp * Kalman.H';
+ dgemm( chn, cht, &L, &M, &L, &one, state->Kp, &L,
+ state->H, &M, &zero, kh->KpH, &L );
+
+ // HKp = Kalman.H * Kalman.Kp;
+ dgemm( chn, chn, &M, &L, &L, &one, state->H, &M,
+ state->Kp, &L, &zero, kh->HKp, &M );
+
+ // HKpH = Kalman.H * KpH
+ dgemm( chn, chn, &M, &M, &L, &one, state->H, &M,
+ kh->KpH, &L, &zero, kh->HKpH, &M );
+
+
+ // % Kalman Gain
+ // Kalman.G = KpH * inv( HKpH + Kalman.Q2 );
+ tmp1 = kh->HKpH;
+ tmp2 = state->Q2;
+ for( n=0; n<M*M; n++ )
+ *(tmp1++) += *(tmp2++);
+ memset( kh->tmp, 0, M*M*sizeof( double ) );
+ deye( M, kh->tmp, 1.0 );
+ dgesv( &M, &M, kh->HKpH, &M, kh->ipiv,
+ kh->tmp, &M, &info );
+ dgemm( chn, chn, &L, &M, &M, &one, kh->KpH, &L,
+ kh->tmp, &M, &zero, state->G, &L );
+
+ // % calculation of the a-posteriori state
+ // % error covariance matrix
+ // K = Kalman.Kp-Kalman.G*HKp;
+ tmp1 = kh->K;
+ tmp2 = state->Kp;
+ for( n=0; n<L*L; n++ )
+ *(tmp1++) = *(tmp2++);
+ dgemm( chn, chn, &L, &L, &M, &minusone, state->G,
+ &L, kh->HKp, &M, &one, kh->K, &L );
+
+
+ // % updating Q1
+ tmp1 = state->Q1;
+ if( mode == 0 )
+ {
+ // no update
+ }
+ else if( mode == 1 )
+ {
+ // Kalman.Q1=diag(diag(K)).*UC;
+ tmp2 = kh->K;
+ for( n=0; n<L*L; n++ )
+ {
+ if( n%(L+1)==0 )
+ {
+ *tmp1 = *tmp2 * UC;
+ tmp2 += L+1;
+ }
+ else
+ *tmp1 = 0;
+ tmp1++;
+ }
+ }
+ else if( mode == 2 )
+ {
+ // %diagonal matrix containing UC
+ // upd = eye(L)/L*UC;
+ // Kalman.Q1=upd*trace(K);
+ tmp2 = kh->K;
+ traceK = *tmp2;
+ for( n=1; n<L; n++ )
+ {
+ tmp2 += L+1;
+ traceK += *tmp2;
+ }
+
+ for( n=0; n<L*L; n++ )
+ {
+ if( n%(L+1)==0 )
+ *tmp1 = traceK * UC / L;
+ else
+ *tmp1 = 0;
+ tmp1++;
+ }
+
+ }
+
+
+ // % a-priori state error covariance matrix
+ // % for the next time step
+ // Kalman.Kp=K+Kalman.Q1;
+ tmp1 = state->Kp;
+ tmp2 = kh->K;
+ tmp3 = state->Q1;
+ for( n=0; n<L*L; n++ )
+ *(tmp1++) = *(tmp2++) + *(tmp3++);
+
+ // % current estimation of state x
+ // Kalman.x = Kalman.x + Kalman.G*err';
+ dgemv( chn, &L, &M, &one, state->G, &L, err,
+ &inc, &one, state->x, &inc );
+
+ // % add new observation, drop oldest
+ // Kalman.yr = [y(:)' Kalman.yr(1:M*(p-1))];
+
+ tmp1 = state->yr + M*p-1;
+ tmp2 = tmp1 - M;
+ for( n=0; n<M*(p-1); n++ )
+ *(tmp1--) = *(tmp2--);
+
+ tmp1 = state->yr;
+ const double *tmpc = y;
+ for( n=0; n<M; n++ )
+ *(tmp1++) = *(tmpc++);
+}
+
+
+
+// ============================
+// Implementation of helper functions
+// ============================
+
+mxArray *eye( int N, double value )
+{
+ mxArray *I;
+ I = mxCreateDoubleMatrix( N, N, mxREAL );
+ deye( N, mxGetPr( I ), value );
+ return I;
+}
+
+void deye( int N, double *i, double value )
+{
+ int n;
+ double *p = i;
+ for( n=0; n<N; n++ )
+ {
+ *p = value;
+ p = p + (N+1);
+ }
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|