zigbee矩阵算法

//Sensor.c

#include "Sensor.h"

/*
矩阵乘法
C = A_T * A =2x2
2x2 x 2x8  = 2x8
2x8 x 8x1  = 2x1
*/


void MatrixMulti_1(float a[2][8],float b[8][2],float c[2][2])
{
int i,j,q;
for(i = 0;i < 2;i++)
for(j = 0;j < 2;j++)
for(q = 0; q < 8;q++)
 c[i][j]  += a[i][q] * b[q][j];  
}
void MatrixMulti_2(float a[2][2],float b[2][8],float c[2][8])
{
int i,j,q;
for(i = 0;i < 2;i++)
for(j = 0;j < 8;j++)
for(q = 0; q < 2;q++)
 c[i][j]  += a[i][q] * b[q][j];  
}
void MatrixMulti_3(float a[2][8],float b[8][1],float c[2][1])
{
int i,j,q;
for(i = 0;i < 2;i++)
for(j = 0;j < 1;j++)
for(q = 0; q < 8;q++)
 c[i][j]  += a[i][q] * b[q][j];  
}
/*
t为转置前的矩阵，k为转置后的矩阵
*/
void MatrixTranspose(float A[N-1][T],float A_T[T][N-1])
{
    int i,j;
    for(i=0;i<N-1;i++)
     for(j=0;j<T;j++)
           A_T[j][i] = A[i][j];


}
/*
计算行列式
按第一行展开计算|A|
*/
float getA(float arcs[T][T],int n)
{
float ans = 0;
    float temp[T][T];
float t;
    int i,j,k;


    if(n==1)
    {
        return arcs[0][0];
    }
    for(i=0;i<n;i++)//原行列式的n列
    {
        for(j=0;j<n-1;j++)//n-1行
        {
            for(k=0;k<n-1;k++)//n-1列
            {
                temp[j][k] = arcs[j+1][(k>=i)?k+1:k];


            }
        }
        t = getA(temp,n-1);
        if(i%2==0)
        {
            ans += arcs[0][i]*t;
        }
        else
        {
            ans -=  arcs[0][i]*t;
        }
    }
    return ans;
}
/*
计算每一行每一列的每个元素所对应的余子式，组成C*
arcs[T][T]原矩阵，n矩阵的级数，ans[T][T]的伴随矩阵
*/
void getAStart(float arcs[T][T],int n,float ans[T][T])
{
int i,j,k,t;
    float temp[T][T];
    if(n==1)
    {
        ans[0][0] = 1;//保证一个元素的矩阵的逆矩阵为其倒数
        return;
    }
    for(i=0;i<n;i++)
    {
        for(j=0;j<n;j++)
        {
            for(k=0;k<n-1;k++)
            {
                for(t=0;t<n-1;t++)
                {
                    temp[k][t] = arcs[k>=i?k+1:k][t>=j?t+1:t];
                }
            }
            ans[j][i]  =  getA(temp,n-1);
            if((i+j)%2 == 1)
            {
                ans[j][i] = - ans[j][i];
            }
        }
    }

}

//Sensor.h

#ifndef SENSOR_H_INCLUDED
#define SENSOR_H_INCLUDED


#define N 9 //9个点
#define K N-1
#define T 2 //两列




float getA(float arcs[T][T],int n);
void getAStart(float arcs[T][T],int n,float ans[T][T]);
void MatrixTranspose(float A[N-1][T],float A_T[T][N-1]);
void MatrixMulti_1(float a[2][8],float b[8][2],float c[2][2]);
void MatrixMulti_2(float a[2][2],float b[2][8],float c[2][8]);
void MatrixMulti_3(float a[2][8],float b[8][1],float c[2][1]);


#endif // SENSOR_H_INCLUDED