Commit 2206fe10 authored by Justin Cano's avatar Justin Cano
Browse files

version 0.1

parents
kalman:
gcc -Wall matrix.c kalman_localization.c test.c -o kalman_demo -lm
clean :
rm -f $(REBUILDABLES)
rm *.o
echo Clean done
File added
#include "kalman_localization.h"
#include "simulation_data.h"
#include "matrix.h"
#include <stdint.h>
#include <math.h>
static const fl tab_posx[6]={2,2,-2,-2};
static const fl tab_posy[6]={-2,2,-2,2};
static const fl tab_posz[6]={0,.5,.5,0};
static const fl SPEED_OF_LIGHT = 299792458.0;
/* EKF gain tuning */
// Measurement noises
#define nu_rho (fl) 0.1
#define nu_gamma (fl) 1.0e-7
// Process noises PSD
#define sigma_x (fl) 4.0
#define sigma_y (fl) 4.0
#define sigma_z (fl) 0.001
#define sigma_delta (fl) 1.0e-8
#define sigma_gamma (fl) 1.0e-6
/* Local shared variables
* */
static matrix x,prx,J,A,Q,P,prP,trJ,K,tempVecColumn,tempVecLine,tempSquare;
static fl r, innov, innov_cov, meas, delta_t;
//static uint32_t previous_time;
//static fl A_delta = 0.01;
static fl Q_psd[5]={sigma_x*sigma_x,sigma_y*sigma_y,sigma_z*sigma_z,sigma_gamma*sigma_gamma,sigma_delta*sigma_delta};
static fl r_gamma = nu_gamma;
static fl r_rho = nu_rho;
// Private functions prototypes
static void fill_matrices(void);
static fl d(uint8_t i);
static fl y(uint8_t i);
static void jacobian_computation(uint8_t);
static void getMeasure(uint8_t);
static void updateMatrices(uint8_t);
static void fill_matrices(){
uint16_t i;
// All matrices are cleared with zeros first
mzeros(x);
mzeros(A);
mzeros(Q);
mzeros(P);
meas = 0.0;
// Initial stepping time (false)
delta_t = 1.0/80.0;
// A = I_8 at first
for(i=0;i<8;i++){
mset(A,1.0,i,i);
}
// We only have information on initial position and drift
mset(x,0.0,1-1,0);
mset(x,0.0,3-1,0);
mset(x,0.0,5-1,0);
mset(x,0.0,7-1,0); // A delta/2
// Initialisation for Q and A
updateMatrices(0);
mscalmul(P,Q,10.0); // incetitude is strong as first
// Synch incertitude is strong
mset(P,1.0e-2,7-1,7-1);
mset(P,1.0,8-1,8-1);
}
static fl d(uint8_t i){
fl res, f;
// x pos
f = mget(prx,1-1,0) - tab_posx[i];
res = f*f;
// y pos
f = mget(prx,3-1,0) - tab_posy[i];
res += f*f;
// z pos
f = mget(prx,5-1,0) - tab_posz[i];
res+= f*f;
// Square root of the sum with a tiny bias to avoid 0 division
res = sqrt(res)+1.0e-9;
return res;
}
static fl y(uint8_t i){
fl res;
if(i==255){
// Skew estimate is the 8th component
res = mget(prx,8-1,0);
}
else
{
// PSEUDORANGE ESTIMATE
// Range estimate
res = d(i);
// Adddition of the clock offset
res += SPEED_OF_LIGHT*mget(prx,7-1,0);
}
return res;
}
static void jacobian_computation(uint8_t i){
uint32_t j;
fl f,dist;
if(i==255){
// Jacobian for the gamma measurement
// 0 for all, except for the last component
for(j=0;j<7;j++){
mset(J,0.0,0,j);
}
mset(J,1.0,0,8-1);
}
else
{
for(j=0;j<4;j++){
// Odd component of the jacobian are set to zero
mset(J,0.0,0,2*i+1);
}
dist = d(i); // We compute predicted distance
f = mget(prx,0,0) - tab_posx[i]; // difference, x axis
mset(J,f/dist,0,0); // Normalization and storage in the 0 component
f = mget(prx,2,0) - tab_posy[i]; // difference, y axis
mset(J,f/dist,0,2); // Normalization
f = mget(prx,4,0) - tab_posz[i]; // difference, z axis
mset(J,f/dist,0,4); // Normalization
mset(J,SPEED_OF_LIGHT,0,6); // The speed of light is added for delta component
}
}
static uint32_t indice_sim;
void set_indice_sim(uint32_t i){
indice_sim = i;
}
static void getMeasure(uint8_t i){
// timestamp and mes
delta_t = 1.0/80.0;
if(i<255){
meas = measures[indice_sim].rho[i];
//printf("pseudorange = %f",meas);
}
else {
meas = measures[indice_sim].gamma;
}
//printf("indice_sim = %d, id = %d, meas= %.8f \n",indice_sim,i,meas);
}
static void updateMatrices(uint8_t indice){
// Set A and Q with respect to the time and r with respect to the measurement
fl dt_2,dt_3;
uint32_t i;
if(indice==255){
r= r_gamma;
}
else{
r=r_rho;
}
for(i=0; i<4; i++){
// Update delta_t in A
mset(A,delta_t,2*i,2*i+1);
}
// True discretization
dt_2 = delta_t*delta_t;
dt_3 = dt_2*delta_t;
dt_2 /=2.0;
dt_3 /=3.0;
for(i=0; i<4; i++){
mset(Q,Q_psd[i]*dt_3,2*i,2*i);
mset(Q,Q_psd[i]*dt_2,2*i+1,2*i);
mset(Q,Q_psd[i]*dt_2,2*i,2*i+1);
mset(Q,Q_psd[i]*delta_t,2*i+1,2*i+1);
}
// Do not forget to add the skew noise
mset(Q,Q_psd[4]*delta_t,6,6);
}
void ekf_init(){
// Memory allocation for all the matricial results
x = createMatrix(8,1);
prx = createMatrix(8,1);
A = createMatrix(8,8);
J = createMatrix(1,8);
trJ = createMatrix(8,1);
P = createMatrix(8,8);
prP = createMatrix(8,8);
Q = createMatrix(8,8);
K = createMatrix(8,1);
tempVecLine = createMatrix(1,8);
tempVecColumn = createMatrix(8,1);
tempSquare = createMatrix(8,8);
// Fill matrices with initial values
fill_matrices();
}
void ekf_loop(uint8_t i){
/* A new measurement is received, update all matrices and proceed with Kalman filtering */
getMeasure(i);
updateMatrices(i);
/* KALMAN FILTER
* Prediction steps */
// x_pred = AX
mmul(prx,A,x);
// P_pred = A P A^t + Q
mtrsp(tempSquare,A); // -> Ts = A^t (local buffer)
mmul(prP,P,tempSquare); // -> prP = P A^t (prP used as second temp matrix)
mmul(tempSquare,A,prP); // -> T_s = A P A^t (Temp square reused)
madd(prP,tempSquare,Q); // -> pr_P = A P A^t + Q
/* Innovation step */
// innov = mes - mes(x)
innov = meas - y(i);
// Jacobian computation
jacobian_computation(i);
// Innovation covariance
// S = J prP J^t + r
mmul(tempVecLine,J,prP); // temp -> J prP (line vec)
innov_cov = mscal(tempVecLine,J);// S = J prP J^t = J.J prP
/* Kalman gain computation */
// K = pr P J^t / S S is a scalar
mtrsp(trJ,J); // -> trJ = J^t
mmul(tempVecColumn,prP,trJ); // -> K = prP trJ
mscalmul(K,tempVecColumn,1.0/innov_cov); // K = pr P J^t/S
/* Prediction steps */
// x = prx + K*innov
mscalmul(tempVecColumn,K,innov); // -> temp = K*innov
madd(x,prx,tempVecColumn); // -> x = prx + K*innov
// P = (I-KJ)prP
mmul(P,K,J); // -> P = KJ
midcomp(tempSquare,P); // -> TS = I - KJ
mmul(P,tempSquare,prP); // -> P = (I-KJ)prP
// Only for debug
estimate.x = mget(x,0,0);
estimate.vx= mget(x,1,0);
estimate.y = mget(x,2,0);
estimate.vy = mget(x,3,0);
estimate.z = mget(x,4,0);
estimate.vz = mget(x,5,0);
estimate.delta = mget(x,6,0);
estimate.gamma = mget(x,7,0);
#define PRINT_MATRICES 0
#if PRINT_MATRICES
mprintmatlab(A,'A');
mprintmatlab(prx,'v');
mprintmatlab(prP,'V');
mprintmatlab(Q,'Q');
printf("innov = %.5f \n",innov);
printf("innov_cov = %.5f \n",innov_cov);
printf("r = %.5f \n",r);
mprintmatlab(J,'J');
mprintmatlab(K,'K');
mprintmatlab(x,'x');
mprintmatlab(P,'P');
#endif
}
#include <stdint.h>
#include "precision_floating.h"
void ekf_init(void);
void ekf_loop(uint8_t);
void set_indice_sim(uint32_t);
function d=dist(v,i)
global posx posy posz
posBalise = [posx(i); posy(i); posz(i)];
posEst = [v(1); v(3); v(5)];
d = norm(posBalise-posEst);
end
\ No newline at end of file
function J = jacobian(v,i)
global posx posy posz speed_of_light
d = dist(v,i);
J = [(v(1)-posx(i))/d 0 (v(3)-posy(i))/d 0 (v(5)-posy(i))/d 0 speed_of_light 0];
end
function rho = pseudorange(v,i)
global speed_of_light
drift = v(7);
distance = dist(v,i);
rho = drift*speed_of_light + distance;
end
File added
%% Out of C kalman (to test)
%% Load new dataset
close all
%load('sim.mat')
rho = {rho0,rho1,rho2,rho3};
global speed_of_light posx posy posz dt
%% Parameters of the simulation
posx = [2,2,-2,-2];
posy = [-2,2,-2,2];
posz = [0,.5,.5,0];
dt = 1/80;
N = length(rho1);
speed_of_light = 299792458;
% Matrices Q, P and A
sigmar = 0.1;
sigmax = 3;
sigmay = 3;
sigmaz = 0.2;
sigmad = 1e-8;
sigmag = 1e-6;
bA = [1 dt; 0 1];
z2 = zeros(2);
A = [bA z2 z2 z2;
z2 bA z2 z2;
z2 z2 bA z2;
z2 z2 z2 bA];
bQ = [dt^3/3 dt^2/2;
dt^2/2 dt];
Q = [bQ*sigmax^2 z2 z2 z2;
z2 bQ*sigmay^2 z2 z2;
z2 z2 bQ*sigmaz^2 z2;
z2 z2 z2 bQ*sigmag^2 + [1 0; 0 0]*sigmad^2]
P = 10*Q + [z2 z2 z2 z2;
z2 z2 z2 z2;
z2 z2 z2 z2;
z2 z2 z2 diag([1e-2,1])];
X = [0 0 0 0 0 0 0 0]';
% Simulation loop
station_id = 1;
% We stack vector
x_vec = zeros(8,N);
inno_vec = zeros(N,1);
for i=1:N
% Prediction
X_pr = A*X;
P_pr = A*P*A'+Q;
% Measurement update and projection in measure space
J = jacobian(X_pr,station_id);
mes = rho{station_id}(i);
pred = pseudorange(X_pr,station_id);
% Innovation
inno = mes - pred;
inno_vec(i) = inno;
S = J*P_pr*J'+sigmar*sigmar;
% Kalman gain computation
K = P_pr*J'/S;
% Updated steps
X = X_pr + K*inno;
P = (eye(8)-K*J)*P_pr;
% Stack estimates
x_vec(:,i)=X;
if station_id == 4
station_id = 1;
else
station_id = station_id + 1;
end
end
figure()
x_true = [x'; vx'; y'; vy'; z'; vz'; delta';gamma1'];
for i=1:8
subplot(2,4,i)
plot(x_vec(i,:))
grid on
hold on
plot(x_true(i,:))
title(num2str(i));
end
figure()
for i=1:8
subplot(2,4,i)
semilogy(abs(x_vec(i,:)-x_true(i,:)))
grid on
hold on
title(num2str(i));
end
figure()
plot(x_true(1,:),x_true(3,:),'k')
hold on
grid on
plot(x_vec(1,:),x_vec(3,:),'.')
#include "matrix.h"
#include "stdio.h"
#include "stdint.h"
#include <stdlib.h>
#include <time.h>
#include <string.h>
// Matrix allocation
matrix createMatrix(uint16_t n_rows,uint16_t n_columns){
matrix M;
M.n_rows = n_rows;
M.n_columns = n_columns;
M.buffer = malloc(n_rows*n_columns*sizeof(fl));
return M;
}
/* Low level functions */
// Coefficient Setter
void mset(matrix M,fl value, uint16_t row, uint16_t column){
M.buffer[row+M.n_rows*column]=value;
}
// Coefficient getter
fl mget(matrix M,uint16_t row, uint16_t column){
return M.buffer[row+M.n_rows*column];
}
// Fill a matrix with zeros
void mzeros(matrix M){
uint16_t i,j;
for(i=0;i<M.n_rows;i++){
for(j=0;j<M.n_columns;j++){
mset(M,0.0,i,j);
}
}
}
/* Test : product validation */
int prodValidation(matrix M, matrix G){
if(M.n_columns == G.n_rows){
// Valid product
return 1;
}
return 0; // Nongood product
}
/* Unar operations for square matrices */
void mtrsp(matrix T, matrix M){
uint16_t i,j;
fl v;
for(i=0;i<M.n_rows;i++){
for(j=0;j<M.n_columns;j++){
v = mget(M,i,j);
mset(T,v,j,i);
}
}
}
/* Binary operation */
/* Copy Copy = Original */
void mcopy(matrix Copy, matrix Original){
uint16_t i,j;
for(i=0;i<Original.n_rows;i++){
for(j=0;j<Original.n_columns;j++){
mset(Copy,mget(Original,i,j),i,j);
}
}
}
/* Addition SUM = A + B */
void madd(matrix SUM, matrix A, matrix B){
uint16_t i,j;
fl v;
for(i=0;i<A.n_rows;i++){
for(j=0;j<A.n_columns;j++){
v = mget(A,i,j)+mget(B,i,j);
mset(SUM,v,i,j);
}
}
}
/* Substraction SUB = A - B */
void msub(matrix SUB, matrix A, matrix B){
uint16_t i,j;
fl v;
for(i=0;i<A.n_rows;i++){
for(j=0;j<A.n_columns;j++){
v = mget(A,i,j)-mget(B,i,j);
mset(SUB,v,i,j);
}
}
}
/* Scalar multiplication RES = A*k */
void mscalmul(matrix RES, matrix A, fl k){
uint16_t i,j;
fl v;
for(i=0;i<A.n_rows;i++){
for(j=0;j<A.n_columns;j++){
v = mget(A,i,j)*k;
mset(RES,v,i,j);
}
}
}
/* Identity complement RES = I_n - M */
void midcomp(matrix RES, matrix M){
uint16_t i,j;
fl v;
for(i=0; i<M.n_rows; i++){
for(j=0; j<M.n_columns;j++){
v= -mget(M,i,j);
if(i==j){
v+= 1.0; // Substraction from id matrix
}
mset(RES,v,i,j);
}
}
}
/* Multiplication MUL = A*B */
void mmul(matrix MUL,matrix A, matrix B){
uint16_t i,j,k,m,n,p;
fl v;
m = A.n_rows;
n = A.n_columns;
p = B.n_columns;
/* Sizes A = [m,n] B = [n,p] C = [m,p] */
for(i=0;i<m;i++){
for(j=0;j<p;j++){
v = 0.0;
for(k=0;k<n;k++){
v+=mget(A,i,k)*mget(B,k,j);
}
mset(MUL,v,i,j);
}
}
}
/* Elementwise product sum (scalar product for vectors) */
fl mscal(matrix A, matrix B){
uint16_t i,j;
fl v = 0.0;
for(i=0;i<A.n_rows;i++){
for(j=0;j<A.n_columns;j++){
v += mget(A,i,j)*mget(B,i,j);
}
}
return v;
}
/* Cholesky inverse
* Caution for sym and definite matrix only
* */
void mchol(matrix INV,matrix A){
// Todo
}
/* Print the matrix in «natural» maths notations */
void mprint(matrix Debug, char name){
uint16_t ii,jj;
char str[5];
str[0]=name;
str[1]='=';
str[2]='\n';
printf(str);
for(ii=0;ii<Debug.n_rows;ii++){
for(jj=0; jj<Debug.n_columns;jj++){
printf("%.8f ",mget(Debug,ii,jj));
}
printf("\n");
}
printf("\n");
}
/* Print the matrix ready to be entered in matlab/scilab (freeware) CLI or script (usefull to debug ;) */
void mprintmatlab(matrix Debug, char name){