## Basic Kalman filter

January 20, 201114 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;

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

/* ------------------------------------------------------------------------- */
// 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);
{
return -1;
}
return 0;
}

/* ------------------------------------------------------------------------- */
// FUNCTION: term
//
// Terminate
//
// RETURN:
/* ------------------------------------------------------------------------- */
int term()
{
{
}
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;
{
return 0;
}
}
}
return 1;
}

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

{
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;
}
}

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);
{
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
{
unsigned int _row = i / col;
unsigned int _col = i % col;
i = _col * row + _row;
BITSET_SET(data, i);
}
while (i != begin);

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;
}``````