root/branch/SENE/sp4a3/sp4a3_kalman_extra.c @ 292
| 1 | jalaffon | #include <stdlib.h>
|
|
#include <stdio.h>
|
|||
#include <math.h>
|
|||
#define error(a) _error(__LINE__,a)
|
|||
void _error(int l,char * r){
|
|||
printf("Erreur Ligne (%d) : %s",l,r);
|
|||
exit(1);
|
|||
}
|
|||
int debug=1;
|
|||
void _Plot_Mat(int n,int m,double in[n][m], const char* name){
|
|||
int i,j;
|
|||
if (!debug) return;
|
|||
printf("\n%s[%d][%d]\n",name,n,m);
|
|||
for (i=0;i<n;i++)
|
|||
{
|
|||
for (j=0;j<m;j++)
|
|||
printf("%f\t",in[i][j]);
|
|||
printf("\n");
|
|||
}
|
|||
}
|
|||
#define Equal_Mat_Mat(Mat_A,Mat_B) _Equal_Mat_Mat(sizeof(Mat_A)/sizeof(Mat_A[0]),sizeof(Mat_A[0])/sizeof(Mat_A[0][0]),Mat_A,sizeof(Mat_B)/sizeof(Mat_B[0]),sizeof(Mat_B[0])/sizeof(Mat_B[0][0]),Mat_B)
|
|||
int _Equal_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double B[nb][mb]){
|
|||
int i,j;
|
|||
if (na!=nb) return 0;
|
|||
if (ma!=mb) return 0;
|
|||
for(i=0;i<na;i++)
|
|||
for(j=0;j<ma;j++){
|
|||
if (fabs(A[i][j]-B[i][j])>1e-6)
|
|||
return 0;
|
|||
}
|
|||
return 1;
|
|||
}
|