?? matrixcalculate.cpp
字號:
// 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);
}
/*矩陣轉(zhuǎn)置*/
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;
}
//坐標(biāo)變換函數(shù)
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;
}
//坐標(biāo)反變換函數(shù)
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}};//狀態(tài)矩陣 C陣
float R_cov[2][2]={{cc,0},{0,cc}};//協(xié)方差矩陣
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];//用于矩陣運(yùn)算的中間變量；
float porce[2][2];//用于矩陣運(yùn)算的中間變量；
float proc[2][2];//用于矩陣運(yùn)算的中間變量；
float ppor[2][2];//用于矩陣運(yùn)算的中間變量；
for(k=1;k<=101;k++)
{
//float C[2][2]={};
/*方差預(yù)測*/
/* 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);*/
/* 狀態(tài)預(yù)測 */
/*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;
}?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -