Révision 461
Ajouté par rademagalh il y a environ 4 ans
| branch/DEMAGALHAES/sp4a3/sp4a3_kalman.c | ||
|---|---|---|
|
|
||
|
void Mul_Mat_Mat(int Ligne_MatA, int Colonne_MatA, double A[Ligne_MatA][Colonne_MatA],int Ligne_MatB, int Colonne_MatB, double B[Ligne_MatB][Colonne_MatB], double Res[Ligne_MatA][Colonne_MatB])
|
||
|
{
|
||
|
for (int i = 0; i < Ligne_MatA; i++)
|
||
|
double Mat_sauv[Ligne_MatB][Colonne_MatB];
|
||
|
for (int i = 0; i < Ligne_MatB; i++)
|
||
|
{
|
||
|
for (int j = 0; j < Colonne_MatB; j++)
|
||
|
{
|
||
|
for (int j = 0; j < Colonne_MatB; j++)
|
||
|
Mat_sauv[i][j]=B[i][j];
|
||
|
}
|
||
|
}
|
||
|
for (int i = 0; i < Ligne_MatA; i++)
|
||
|
{
|
||
|
for (int j = 0; j < Colonne_MatB; j++)
|
||
|
{
|
||
|
Res[i][j]=0;
|
||
|
for(int k=0; k < Ligne_MatB; k++)
|
||
|
{
|
||
|
Res[i][j]=0;
|
||
|
for(int k=0; k < Ligne_MatB; k++)
|
||
|
{
|
||
|
Res[i][j] += A[i][k]*B[k][j];
|
||
|
}
|
||
|
Res[i][j] += A[i][k]*Mat_sauv[k][j];
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
void Inverse_Mat_Mat(int Ligne_MatA, int Colonne_MatA, double A[Ligne_MatA][Colonne_MatA],double Res[Ligne_MatA][Colonne_MatA])
|
||
| ... | ... | |
|
{
|
||
|
t -= t0;x -= x0;y -= y0;
|
||
|
|
||
|
debug=1; ///Mettre à 1 pour afficher les matrices.
|
||
|
debug=0; ///Mettre à 1 pour afficher les matrices.
|
||
|
///Ajouter votre code ci-dessous///
|
||
|
// Kalmans
|
||
|
|
||
Formats disponibles : Unified diff
Fin de Kalman avec les bonnes courbes