Révision 246
Ajouté par nibenmbare il y a presque 4 ans
| branch/BEN_MBAREK/sp4a3/sp4a3_kalman.c | ||
|---|---|---|
|
#include "sp4a3_kalman_extra.h"
|
||
|
|
||
|
|
||
|
void Add_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double b[nb][mb], double out[na][ma]){
|
||
|
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 = ((A[0][0])*(A[1][1])-(A[1][0])*(A[0][1]));
|
||
|
B[0][0] = (1/d)*A[1][1];
|
||
|
B[0][1] = (1/d)*(-A[0][1]);
|
||
|
B[1][0] = (1/d)*(-A[1][0]);
|
||
|
B[1][1] = (1/d)*A[0][0];
|
||
|
|
||
|
|
||
|
}
|
||
|
|
||
| ... | ... | |
|
for (i=0;i<n;i++)
|
||
|
for (j=0;j<m;j++)
|
||
|
R[j][i]=A[i][j];
|
||
|
}
|
||
|
}
|
||
|
|
||
|
void Sub_Mat_Mat(int na,int ma,double A[na][ma],int nb,int mb,double b[nb][mb], double out[na][ma]){
|
||
|
|
||
|
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]){
|
||
|
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 i,j,k;
|
||
|
|
||
|
for(i=0;i<na;i++)
|
||
|
{
|
||
|
for(j=0;j<mb;j++)
|
||
|
{
|
||
|
out[i][j] = 0;
|
||
|
}
|
||
|
}
|
||
|
for (i=0;i<na;i++)
|
||
|
{
|
||
|
for(j=0;j<mb;j++)
|
||
|
{
|
||
|
for (k=0;k<ma;k++)
|
||
|
{
|
||
|
out[i][j] = A[i][k]*B[k][j] + out[i][j];
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
}
|
||
|
|
||
Formats disponibles : Unified diff
tp3sp