Révision 247
Ajouté par beboucheix il y a presque 4 ans
branch/boucheix/sp4a3/sp4a3_kalman.c | ||
---|---|---|
|
||
|
||
void Add_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double b[nb][mb], double out[na][ma]){
|
||
|
||
int i,j;
|
||
for (i=0;i<na;i++)
|
||
for (j=0;j<ma;j++)
|
||
out[i][j]=A[i][j]+b[i][j];
|
||
}
|
||
|
||
void Inverse_Mat_22(int n,int m,double A[n][m],double B[n][m]){
|
||
void Inverse_Mat_22(int n,int m,double A[n][m],double B[n][m]){
|
||
float d;
|
||
|
||
d=1/((A[0][0]*A[1][1])-(A[0][1]*A[1][0]));
|
||
|
||
B[0][0]=d*A[1][1];
|
||
B[1][1]=d*A[0][0];
|
||
B[0][1]=-d*A[0][1];
|
||
B[1][0]=-d*A[1][0];
|
||
|
||
|
||
|
||
|
||
}
|
||
|
||
... | ... | |
}
|
||
|
||
void Sub_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double b[nb][mb], double out[na][ma]){
|
||
|
||
int i,j;
|
||
for (i=0;i<na;i++){
|
||
for (j=0;j<ma;j++){
|
||
out[i][j]=A[i][j]-b[i][j];
|
||
}
|
||
}
|
||
}
|
||
|
||
void Mul_Mat_Mat(int na,int ma,double A[na][ma], int nb,int mb,double B[nb][mb], double out[na][mb]){
|
||
|
||
int j,i,k;
|
||
for(i=0;i<na;i++)
|
||
{
|
||
|
||
for(j=0;j<mb;j++)
|
||
{
|
||
|
||
out[i][j]=0;
|
||
for(k=0;k<ma;k++)
|
||
{
|
||
|
||
out[i][j]+=A[i][k]*B[k][j];
|
||
|
||
}}}
|
||
}
|
||
|
||
|
||
... | ... | |
double RTT44[4][4]={{1,6,9,5},{2,5,8,4},{7,7,7,3},{4,8,6,2}};
|
||
double RMT44T41[4][1]={{-2387},{-4171},{-3585},{-1321}};
|
||
double RMT44T42[4][2]={{-308,-268},{-611,-99},{-816,118},{-432,122}};
|
||
double RAT44[4][4]={{13,15,21,19},{27,27,30,48},{87,53,19,9},{59,14,15,49}};
|
||
double RAT44[4][4]={{13,15,21,19},{27,27,30,48},{87,53,19,9},{59,14,15,49}};
|
||
|
||
double RST44[4][4]={{-11,-11,-7,-11},{-15,-17,-16,-32},{-69,-37,-5,3},{-49,-6,-9,-45}};
|
||
double RMT44T44[4][4]={{816,412,192,304},{1155,583,379,687},{1146,668,466,758},{486,308,222,338}};
|
||
|
Formats disponibles : Unified diff
TP3 programmes transposée addition soustraction inverse et multiplication