matrixcalculate.cpp

nios 计算卡尔曼滤波程序

// matrixcalculate.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include "malloc.h" 
#include "math.h"
#include <stdlib.h> 
#include <stdio.h> 
#define q1 0.5
#define pi 3.14159
#define T_step 0.1
#define H_period 2*pi
#define cc 0.1
int matrixinv(float c[], float a[],int n) 
{ 
	int *is,*js,i,j,k,l,u,v; 
    float d,p; 
    is=(int *)malloc(n*sizeof(int)); 
    js=(int *)malloc(n*sizeof(int)); 
	for (i=0; i<n; i++) 
	{
		for(j=0;j<n;j++)

	       a[i*n+j]=c[i*n+j];
	}
    for (k=0; k<=n-1; k++) 
	{
		d=0.0; 
        for (i=k; i<=n-1; i++) 
        for (j=k; j<=n-1; j++) 
		{ 	l=i*n+j; p=fabs(a[l]); 
			if (p>d) 
			{
				d=p; is[k]=i; js[k]=j;
			} 
		} 
        if (d+1.0==1.0) 
		
		{
		    free(is); free(js); printf("err**not inv\n"); 
			return(0); 
		} 
        if (is[k]!=k) 
        for (j=0; j<=n-1; j++) 
		{ 
			u=k*n+j; v=is[k]*n+j; 
            p=a[u]; a[u]=a[v]; a[v]=p; 
		} 
        if (js[k]!=k) 
        for (i=0; i<=n-1; i++) 
		{ 
			u=i*n+k; v=i*n+js[k]; 
            p=a[u]; a[u]=a[v]; a[v]=p; 
		} 
        l=k*n+k; 
        a[l]=1.0/a[l]; 
        for (j=0; j<=n-1; j++) 
        if (j!=k) 
		{ 
			u=k*n+j; a[u]=a[u]*a[l];
		} 
        for (i=0; i<=n-1; i++) 
        if (i!=k) 
        for (j=0; j<=n-1; j++) 
        if (j!=k) 
		{ 
			u=i*n+j; 
            a[u]=a[u]-a[i*n+k]*a[k*n+j]; 
		} 
        for (i=0; i<=n-1; i++) 
        if (i!=k) 
		{ 
			u=i*n+k; a[u]=-a[u]*a[l];
		} 

	} 
        for (k=n-1; k>=0; k--) 
		{ 
	        if (js[k]!=k) 
            for (j=0; j<=n-1; j++) 
			{ 
				u=k*n+j; v=js[k]*n+j; 
                p=a[u]; a[u]=a[v]; a[v]=p; 
			} 
            if (is[k]!=k) 
            for (i=0; i<=n-1; i++) 
			{ 
				 u=i*n+k; v=i*n+is[k]; 
                 p=a[u]; a[u]=a[v]; a[v]=p; 
			} 
		}  
            free(is); free(js); 
            return(1); 
} 
/*矩阵转置*/
int atv(float *in,float *out,int n) 
{ 
int i,j; 

for(i=0;i<n;i++) 
{ 
   for(j=0;j<n;j++) 
   *(out+ j*n +i)=*(in+ i*n +j); 
} 
return 1; 
} 
/*矩阵乘法*/
int abv(float *matrixa,float *matrixb,float *out,int am,int an,int bm,int bn)
{ 
int i,j,k; 
float temp; 
if(an!=bm) {return 0;} /*矩阵不可以计算*/ 
for(i=0;i<am;i++) 
{ 
    for(j=0;j<bn;j++) 
    { 
      for(k=0,temp=0;k<an;k++) 
      temp=temp + *(matrixa+i*an+k) * *(matrixb+k*bn+j); 
      *(out+i*bn+j) = temp; 
    } 

} 
return 1; 
} 
void initiones(float *q,int n) /*形成单位矩阵*/
{ 
int i,j; 
for(i=0;i<n;i++) 
    for(j=0;j<n;j++) 
    { if(i==j) 
           q[i*n+j]=1; 
       else 
           q[i*n+j]=0; 
    } 
}
/*求矩阵和,a,b为输入阵,out为和的矩阵,m,n为行和列*/ 
int abaddv(float *matrixa,float *matrixb,float *out,int m,int n)
{ 
int i,j;
for(i=0;i<m;i++)
{
   for(j=0;j<n;j++)
    *(out+i*n+j)=*(matrixa+i*n+j)+*(matrixb+i*n+j);
}
return 1;
}
/*求矩阵差,a,b为输入阵,out为差的矩阵,m,n为行和列*/ 
int absubv(float *matrixa,float *matrixb,float *out,int m,int n)
{ 
int i,j;
for(i=0;i<m;i++)
{
   for(j=0;j<n;j++)
    *(out+i*n+j)=*(matrixa+i*n+j)-*(matrixb+i*n+j);
}
return 1;
}
//坐标变换函数
float *cordswitch(float b_h,float b_p,float b_r,float b_a,float b_e,float *b_ac,float *b_ec)
{
    (*b_ac)=atan((cos(b_e)*(cos(b_r)*sin(b_a-b_h)+sin(b_r)*sin(b_p)*cos(b_a-b_h))-sin(b_e)*sin(b_r)*cos(b_p))/(cos(b_e)*cos(b_p)*cos(b_a-b_h)+sin(b_e)*sin(b_p)));
	(*b_ec)=asin(cos(b_e)*(sin(b_r)*sin(b_a-b_h)-cos(b_r)*sin(b_p)*cos(b_a-b_h))+sin(b_e)*cos(b_r)*cos(b_p));
	return b_ec;
}
//坐标反变换函数
float *anticorswitch(float b_h,float b_p,float b_r,float b_ac,float b_ec,float *b_a,float *b_e)
{
    (*b_a)=atan((sin(b_ec)*sin(b_r)+cos(b_ec)*cos(b_r)*sin(b_ac))/(cos(b_ec)*(cos(b_p)*cos(b_ac)+sin(b_p)*sin(b_r)*sin(b_ac))-sin(b_ec)*sin(b_p)*cos(b_r)))+b_h; 
	(*b_e)=asin(cos(b_ec)*(sin(b_p)*cos(b_ac)-cos(b_p)*sin(b_r)*sin(b_ac))+sin(b_ec)*cos(b_p)*cos(b_r));
    return b_a;
}
main()
{
	FILE *fp;
	char buf[64];
	float database[101]={0.0000,0.0628,0.1253,0.1874,0.2487,0.3090,0.3681,0.4258,0.4818,
    0.5358,0.5878,0.6374,0.6845,0.7290,0.7705,0.8090,0.8443,0.8763,0.9048,0.9298,
    0.9511,0.9686,0.9823,0.9921,0.9980,1.0000,0.9980,0.9921,0.9823,0.9686,0.9511,
    0.9298,0.9048,0.8763,0.8443,0.8090,0.7705,0.7290,0.6845,0.6374,0.5878,0.5358,0.4818,
    0.4258,0.3681,0.3090,0.2487,0.1874,0.1253,0.0628,0.0000,-0.0628,-0.1253,-0.1874,-0.2487,
    -0.3090,-0.3681,-0.4258,-0.4818,-0.5358,-0.5878,-0.6374,-0.6845,-0.7290,-0.7705,-0.8090,
    -0.8443,-0.8763,-0.9048,-0.9298,-0.9511,-0.9686,-0.9823,-0.9921,-0.9980,-1.0000,-0.9980,
    -0.9921,-0.9823,-0.9686,-0.9511,-0.9298,-0.9048,-0.8763,-0.8443,-0.8090,-0.7705,-0.7290,
    -0.6846,-0.6374,-0.5878,-0.5358,-0.4818,-0.4258,-0.3681,-0.3090,-0.2487,-0.1874,-0.1253,
    -0.0628,-0.0000};
	float datacont[101];
	float position;
	float velocy;
	int k=1;
	int i,j=0;
	float KK[2][2]={{0,0},{0,0}};
	float P_pre[2][2]={{0,0},{0,0}};
	float Fai_Matrix[2][2]={{1,T_step},{0,1}};
	float H_Matrix[2][2]={{1,0},{0,1}};//状态矩阵  C阵
	float R_cov[2][2]={{cc,0},{0,cc}};//协方差矩阵
	float Q[2][2]={{q1,0},{0,q1}};
	float P_est[2][2]={{cc,cc/T_step},{cc/T_step,cc}};
	float MI[2][2]={{1,0},{0,1}};
	//float C[2][2]={{1,0},{0,1}};
	float XK[2][1]={{0},{0}};
	float State_Pre[2][1]={{0.5},{0.01}};
    //float State_Pre[2][1];
    float State_est[2][1]={{0},{0}};
	float poop[2][1]={{0},{0}};
	float popo[2][1]={{0},{0}};
    //float Mea_error[2][2];
   // float Mea_noisytraj[2][2];
   // float Mea_Pre[2][2];
	float process[2][2];//用于矩阵运算的中间变量；
    float porce[2][2];//用于矩阵运算的中间变量；
    float proc[2][2];//用于矩阵运算的中间变量；
    float ppor[2][2];//用于矩阵运算的中间变量；
for(k=1;k<=101;k++)
{
	//float C[2][2]={};
 /*方差预测*/
  /*  P_pre=Fai_Matrix*P_est*Fai_Matrix'+ Q;  */
abv(*Fai_Matrix,*P_est,*process,2,2,2,2);            //process=Fai_Matrix*Pcov_est;
atv(*Fai_Matrix,*porce,2);                              //porce=Fai_Matrix';
abv(*process,*porce,*proc,2,2,2,2);                     //proc=Fai_Matrix*Pcov_est*Fai_Matrix'
abaddv(*proc,*Q,*P_pre,2,2);                            //得到P_pre=Fai_Matrix*Pcov_est*Fai_Matrix'+ Q;
	/*增益计算*/
   /* K=P_pre*H_Matrix'*inv(H_Matrix*P_pre*H_Matrix'+R_cov) ;   
	Mea_error=Mea_noisytraj-Mea_Pre;*/
abv(*H_Matrix,*P_pre,*process,2,2,2,2);                 //process=H_Matrix*P_pre;
atv(*H_Matrix,*ppor,2);                                 //ppor=H_Matrix';
abv(*process,*ppor,*porce,2,2,2,2);                     //porce=H_Matrix*P_pre*H_Matrix';
abaddv(*porce,*R_cov,*process,2,2);                     //process=H_Matrix*P_pre*H_Matrix'+R_cov;
matrixinv(*process,*porce,2);                           //porce=inv(H_Matrix*P_pre*H_Matrix'+R_cov);
abv(*P_pre,*ppor,*process,2,2,2,2);                     //process=P_pre*H_Matrix';
abv(*process,*porce,*KK,2,2,2,2);                        //得到K阵=P_pre*H_Matrix'*inv(H_Matrix*P_pre*H_Matrix'+R_cov) ;
//absubv(*Mea_noisytraj,*Mea_Pre,*Mea_error,2,2);         //得到Mea_error阵=Mea_noisytraj-Mea_Pre;
    /*方差计算*/
    /*   P_est=(eye(2)-K*H_Matrix)*P_pre;   */
abv(*KK,*H_Matrix,*proc,2,2,2,2);                        //proc=K*H_Matrix;
absubv(*MI,*proc,*porce,2,2);                           //porce=eye(3)-K*H_Matrix;
abv(*porce,*P_pre,*P_est,2,2,2,2);                      //process=(eye(3)-K*H_Matrix)*P_pre;
/*abv(*porce,*P_pre,*ppor,2,2,2,2);   
//改变p_est计算公式，增加P_est=(eye(2)-K*H_Matrix)*P_pre*(eye(2)-K*H_Matrix)'+KK*R_cov*KK';
atv(*porce,*proc,2);
abv(*ppor,*proc,*process,2,2,2,2);
abv(*KK,*R_cov,*ppor,2,2,2,2);
atv(*KK,*proc,2);
abv(*ppor,*proc,*porce,2,2,2,2);
abaddv(*process,*porce,*P_est,2,2);*/
    /* 状态预测 */
/*State_est=Fai_Matrix*State_Pre+K*(X-H_Matrix*Fai_Matrix*State_Pre);*/
velocy=(database[k]-database[k-1])/T_step;
position=database[k];
XK[0][0]=position;
XK[1][0]=velocy;
//XK[2][1]={{position},{velocy}};
abv(*H_Matrix,*Fai_Matrix,*process,2,2,2,2);
abv(*process,*State_Pre,*poop,2,2,2,1);
absubv(*XK,*poop,*popo,2,1);
abv(*KK,*popo,*poop,2,2,2,1);
abv(*Fai_Matrix,*State_Pre,*popo,2,2,2,1);
abaddv(*popo,*poop,*State_est,2,1);
datacont[k-1]=State_est[0][0];
for(i=0;i<2;i++)
{
	for(j=0;j<1;j++)
	{
	    State_Pre[i][j]=State_est[i][j];
	}
}
}
  
//	float a[102];
	
//	fp=fopen("sin.txt","w+");
	fp=fopen("sindata.txt","w+");
	//fwrite(a, sizeof(float) * 102, 1, fp);
	for(j=0;j<=100;j++)
	{
		for(i = 0; i < 64; i++)
			buf[i] = '\0';
		sprintf(buf, "%.4f\n", datacont[j]);
		fwrite(buf, sizeof(buf), 1, fp);
	}
	fclose(fp);  
	return 0;
}