DSPRelated.com
Code
   Code Snippets    Submitted by slvn

Basic Kalman filter

● January 20, 2011●14 comments ● Coded in Scilab 
// Clear initialisation 
clf()

// Time vector
t =[0:0.5:100];
len_t = length(t);
dt = 100 / len_t;

// Real values x: position v: velocity a: acceleration
x_vrai = 0 * t;
v_vrai = 0 * t;
a_vrai = 0 * t;

// Meseared values.
x_mes = 0 * t;
v_mes = 0 * t;
a_mes = 0 * t;

// Initialisation
for i = 0.02 * len_t: 0.3 * len_t,
  a_vrai(i) = 1;
end

for i = 0.5 * len_t: 0.7 * len_t,
  a_vrai(i) = 2;
end

// State of kalman filter
etat_vrai = [ x_vrai(1);  v_vrai(1); a_vrai(1) ];

A = [ 1 dt dt*dt/2; 0 1 dt; 0 0 1];//transition matrice
for i = 2:length(t),
  etat_vrai = [ x_vrai(i-1);  v_vrai(i-1); a_vrai(i) ];
  etat_vrai = A * etat_vrai;
  x_vrai(i) = etat_vrai(1);  
  v_vrai(i) = etat_vrai(2);
end

max_brt_x = 200;
max_brt_v = 10;
max_brt_a = 0.3;

// Random noise
x_brt = grand(1, len_t, 'unf', -max_brt_x, max_brt_x);
v_brt = grand(1, len_t, 'unf', -max_brt_v, max_brt_v);
a_brt = grand(1, len_t, 'unf', -max_brt_a, max_brt_a);

// Compute meas
x_mes = x_vrai + x_brt;
v_mes = v_vrai + v_brt;
a_mes = a_vrai + a_brt;

// START 
x_est = 0 * t;
v_est = 0 * t;
a_est = 0 * t;

etat_mes = [ 0; 0; 0 ];
etat_est = [ 0; 0; 0 ];
inn = [ 0; 0; 0 ];
// Matrix of covariance of the bruit.
// independant noise -> null except for diagonal, 
// and on the diagonal == sigma^2.
// VAR(aX+b) = a^2 * VAR(X);
// VAR(X) = E[X*X] - E[X]*E[X]
R = [ max_brt_x*max_brt_x 0 0; 0 max_brt_v*max_brt_v 0; 0 0 max_brt_a*max_brt_a ];

Q = [1 0 0; 0 1 0; 0 0 1];
P = Q;
for i = 2:length(t),
  // Init : measured state
  etat_mes = [ x_mes(i);  v_mes(i); a_mes(i) ];
  
  // Innovation: measured state -  estimated state
  inn = etat_mes - etat_est;
  
  // Covariance of the innovation
  S = P + R;
  
  // Gain
  K = A * inv(S);
  
  // Estimated state
  etat_est = A * etat_est + K * inn;
  
  // Covariance of prediction error
  // C = identity
  P = A * P * A' + Q - A * P * inv(S) * P * A';
  
  // End: estimated state
  x_est(i) = etat_est(1);
  v_est(i) = etat_est(2); 
  a_est(i) = etat_est(3); 

end

// Blue : real
// Gree : noised
// Red: estimated
subplot(311)
plot(t, a_vrai, t, a_mes, t, a_est);  
subplot(312)
plot(t, v_vrai, t, v_mes, t, v_est);  
subplot(313)
plot(t, x_vrai, t, x_mes, t, x_est);

Inplace matrix transposition

● November 3, 2010 ● Coded in C++ 
/* ------------------------------------------------------------------------- */
//
// This C++ code intents to do inplace transposition for matrix.
// It is more efficient for huge matrix, and quite generic as it uses classes 
// and templates.
//
// Feel free to use it and to modify it.
// Allocation can be improved according to your system.
// Compiles with g++
/* ------------------------------------------------------------------------- */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <alloca.h>
#include <iostream>

using std::swap;
using std::cout;

template <typename T>
class matrix_t
{
   public:
      unsigned int row;
      unsigned int col;
      T *addr;

      /* ------------------------------------------------------------------------- */
      // FUNCTION: matrix_t 
      /* ------------------------------------------------------------------------- */
      matrix_t()
      {
         row = 0;
         col = 0;
         addr = NULL;
      }

      /* ------------------------------------------------------------------------- */
      // FUNCTION: init 
      // 
      // PARAM: _row
      // PARAM: _col
      // 
      // Init with allocation
      //
      // RETURN:
      /* ------------------------------------------------------------------------- */
      int init(unsigned int _row, unsigned int _col)
      {
         row = _row;
         col = _col;
         addr = (T *) calloc(sizeof(T), row * col);
         if (addr == NULL)
         {
            return -1;
         }
         return 0;
      }

      /* ------------------------------------------------------------------------- */
      // FUNCTION: term 
      // 
      // Terminate
      //
      // RETURN:
      /* ------------------------------------------------------------------------- */
      int term()
      {
         if (addr)
         {
            free(addr);
         }
         addr = NULL;
         return 0;
      }

      /* ------------------------------------------------------------------------- */
      // FUNCTION: fill 
      //
      // Fill with values
      /* ------------------------------------------------------------------------- */
      void fill()
      {
         unsigned int x = 0;
         int c = 0;
         for (x = 0; x < row; x++)
         {
            unsigned int y = 0;
            for (y = 0; y < col; y++)
            {
               addr[x * col + y] = (T) c++;
            }
         }
      }

      /* ------------------------------------------------------------------------- */
      // FUNCTION: print 
      //
      // Print to standard output
      /* ------------------------------------------------------------------------- */
      void print()
      {
         unsigned int x = 0;
         cout << "row: " << row << "\n";
         cout << "col: " << col << "\n";
         for (x = 0; x < row; x++)
         {
            unsigned int y = 0;
            for (y = 0; y < col; y++)
            {
               cout.width(3);
               cout << addr[x * col + y] << " ";
            }
            cout << "\n";
         }
         cout << "\n";
         cout << "\n";
      }

      /* ------------------------------------------------------------------------- */
      // FUNCTION: operator== 
      // 
      // PARAM: mat2
      //
      // Comparaison of two matrices
      // 
      // RETURN: 1 if equals; 0 if differents
      /* ------------------------------------------------------------------------- */
      int operator==(matrix_t mat2)
      {
         unsigned int x = 0;

         if (row != mat2.row)
         {
            fprintf(stderr, "Error : different number of rows (%d != %d)\n", row, mat2.row);
            return 0;
         }

         if (col != mat2.col)
         {
            fprintf(stderr, "Error : different number of columns (%d != %d)\n", col, mat2.col);
            return 0;
         }
         
         for (x = 0; x < row; x++)
         {
            unsigned int y = 0;
            for (y = 0; y < col; y++)
            {
               unsigned int pos = x * col + y;
               if (addr[pos] != mat2.addr[pos])
               {
                  return 0;
               }
            }
         }
         return 1;
      }

      /* ------------------------------------------------------------------------- */
      // FUNCTION: transpose 
      // 
      // Transposition
      /* ------------------------------------------------------------------------- */
      int transpose()
      {
         unsigned int x = 0;
         T *addr2 = (T *) calloc(sizeof(T), row * col);
         
         if (addr2 == NULL)
         {
            return -1;
         }

         for (x = 0; x < row; x++)
         {
            unsigned int y = 0;
            for (y = 0; y < col; y++)
            {
               int s = x * col + y;
               int d = y * row + x;
               addr2[d] = addr[s];
            }
         }

         free(addr);
         addr = addr2;
         swap<unsigned int>(row, col);
         return 0;
      }

#define BITSET_POS(ptr,x)   ((int)(((unsigned char*)(ptr))[(x)/8]))
#define BITSET_IS(ptr,x)   ((int)(((unsigned char*)(ptr))[(x)/8]) & (int)(1<<((x)&7)))
#define BITSET_SET(ptr,x)   ((unsigned char*)(ptr))[(x)/8] |= (unsigned char)(1<<((x)&7))

      /* ------------------------------------------------------------------------- */
      // FUNCTION: transpose_inplace 
      //  
      // Inplace transposition
      /* ------------------------------------------------------------------------- */
      int transpose_inplace()
      {
         unsigned int len = row * col;
         unsigned int size = (len/8) + 1;
         unsigned int i = 0;
         unsigned char *data = (unsigned char*) calloc(1, size);
         if (addr == NULL)
         {
            return -1;
         }
         
         while (i < len)
         {
            if (BITSET_POS(data, i) == 0xff)
            {
               i += 8;
               continue;
            }

            if (BITSET_IS(data, i))
            {
               i++;
               continue;
            }

            unsigned int begin = i;
            BITSET_SET(data, i);
            T tmp = (T)0;
            do
            {
               swap<T>(addr[i], tmp);
               unsigned int _row = i / col;
               unsigned int _col = i % col;
               i = _col * row + _row;
               BITSET_SET(data, i);
            }
            while (i != begin);

            swap<T>(addr[i], tmp);
            i = begin + 1;
         }

         free(data);
         swap<unsigned int>(row, col);
         return 0;
      }
}; 

/* ------------------------------------------------------------------------- */
// FUNCTION: main 
// 
// Test case : 
//    transpose a 9x23 matrix
//    transpose inplace a 9x23 matrix
//    and compare
//
/* ------------------------------------------------------------------------- */
int main()
{
   matrix_t<double> m1, m1_ref;

   m1.init(9, 23);
   m1_ref.init(9, 23);

   m1.fill();
   m1_ref.fill();

   m1.print();

   m1.transpose_inplace();
   m1_ref.transpose();

   m1.print();

   if (m1 == m1_ref)
   {
      printf("==\n");
   }
   else
   {
      printf("!=\n");
   }
   
   m1.term();
   m1_ref.term();

   return 0;
}