?? operator.cpp.svn-base
字號:
#include "operator.h"
Matrix* initMatrix(int x,int y)
{
int i;
Matrix* mat;
mat=new Matrix;
mat->row=x;
mat->column=y;
mat->data=new float [x*y];
for(i=0;i<x*y;i++)
*(mat->data+i)=0;
return mat;
}
Kalman* initKalman(int D,int M,int C)
{
int i,j;
Kalman* kal;
kal=new Kalman;
kal->DP=D;
kal->MP=M;
kal->CP=C;
kal->lastP=initMatrix(D,D);//waiting for a better value instead of 0
kal->lastState=initMatrix(D,1);//waiting for a better value instead of 0
kal->preState=initMatrix(D,1);
kal->modifyState=initMatrix(D,1);
kal->contol=initMatrix(C,1);
kal->measurement=initMatrix(M,1);
kal->P_pre=initMatrix(D,D);
kal->P_modify=initMatrix(D,D);
kal->Kg=initMatrix(D,M);
kal->A=initMatrix(D,D);//waiting for a better value instead of 1
kal->B=initMatrix(D,C);
kal->H=initMatrix(M,M);//waiting for a better value instead of 1
kal->Q=initMatrix(D,D);//waiting for a better value instead of 0
kal->R=initMatrix(M,M);//waiting for a better value instead of 0
kal->I=initMatrix(M,M);
for(i=0;i<M;i++)
for(j=0;j<M;j++)
if(i==j)
kal->I->data[i*M+j]=1;
return kal;
}
void releaseMatrix(Matrix* mat)
{
delete mat->data;
delete mat;
}
void releaseKalman(Kalman* kal)
{
releaseMatrix(kal->lastP);
releaseMatrix(kal->lastState);
releaseMatrix(kal->preState);
releaseMatrix(kal->modifyState);
releaseMatrix(kal->contol);
releaseMatrix(kal->measurement);
releaseMatrix(kal->P_pre);
releaseMatrix(kal->P_modify);
releaseMatrix(kal->Kg);
releaseMatrix(kal->A);
releaseMatrix(kal->B);
releaseMatrix(kal->H);
releaseMatrix(kal->Q);
releaseMatrix(kal->R);
releaseMatrix(kal->I);
delete kal;
}
Matrix* matMultiple(Matrix* mat1,Matrix* mat2)
{
int i,j,k;
if(mat1->column==mat2->row)
{
Matrix*temp;
temp=initMatrix(mat1->row,mat2->column);
for(i=0;i<mat1->row;i++)
{
for(j=0;j<mat2->column;j++)
{
for(k=0;k<mat1->column;k++)
{
*(temp->data+i*mat2->column+j)+=(*(mat1->data+i*mat1->column+k))*(*(mat2->data+k*mat2->column+j));
}
}
}
return temp;
}
else
{
printf("ERROR:Can't computing for the size of the two mat error!\n");
return NULL;
}
}
Matrix* matDiff(Matrix* mat1,Matrix* mat2)
{
int i,j;
if((mat1->row==mat2->row)&&(mat1->column==mat2->column))
{
Matrix* temp;
temp=initMatrix(mat1->row,mat2->column);
for(i=0;i<mat1->row;i++)
{
for(j=0;j<mat2->column;j++)
{
temp->data[i*mat1->column+j]=mat1->data[i*mat1->column+j]/mat2->data[i*mat2->column+j];
}
}
return temp;
}
else
{
printf("ERROR:Can't computing for the size of the two mat error!\n");
return NULL;
}
}
Matrix* matPlus(Matrix* mat1,Matrix* mat2)
{
int i,j;
if((mat1->row==mat2->row)&&(mat1->column==mat2->column))
{
Matrix* temp;
temp=initMatrix(mat1->row,mat2->column);
for(i=0;i<mat1->row;i++)
{
for(j=0;j<mat2->column;j++)
{
temp->data[i*mat1->column+j]=mat1->data[i*mat1->column+j]+mat2->data[i*mat2->column+j];
}
}
return temp;
}
else
{
printf("ERROR:Can't computing for the size of the two mat error!\n");
return NULL;
}
}
Matrix* matMinus(Matrix* mat1,Matrix* mat2)
{
int i,j;
if((mat1->row==mat2->row)&&(mat1->column==mat2->column))
{
Matrix* temp;
temp=initMatrix(mat1->row,mat2->column);
for(i=0;i<mat1->row;i++)
{
for(j=0;j<mat2->column;j++)
{
temp->data[i*mat1->column+j]=mat1->data[i*mat1->column+j]-mat2->data[i*mat2->column+j];
}
}
return temp;
}
else
{
printf("ERROR:Can't computing for the size of the two mat error!\n");
return NULL;
}
}
Matrix* matTranspose(Matrix* mat)
{
int i,j;
if(mat->row==mat->column)
{
Matrix* temp;
temp=initMatrix(mat->row,mat->column);
for(i=0;i<mat->row;i++)
{
for(j=0;j<mat->column;j++)
{
temp->data[j*mat->row+i]=mat->data[i*mat->column+j];
}
}
return temp;
}
else
{
printf("ERROR:Can't computing for the size of the two mat error!\n");
return NULL;
}
}
void valuechange(Matrix* mat1,Matrix* mat2)
{
if((mat1->row==mat2->row)&&(mat1->column==mat2->column))
{
int i;
for(i=0;i<mat1->row*mat2->column;i++)
mat1->data[i]=mat2->data[i];
}
else
printf("ERROR:Can't computing for the size of the two mat error!\n");
}
float DeterminantValue(Matrix* mat)
{
if((mat->column==mat->row)&&(mat->column==2))
{
return mat->data[0]*mat->data[3]-mat->data[1]*mat->data[2];
}
else
if((mat->column==mat->row)&&(mat->column==3))
{
return mat->data[0]*mat->data[4]*mat->data[8]+mat->data[2]*mat->data[3]*mat->data[7]+mat->data[1]*mat->data[5]*mat->data[6]-mat->data[2]*mat->data[4]
*mat->data[6]-mat->data[1]*mat->data[3]*mat->data[8]-mat->data[0]*mat->data[5]*mat->data[7];
}
else
printf("ERROR:Can't computing for the size of the two mat error!\n");
}
Matrix* getLeftMatrix(Matrix* mat,int ro ,int co)
{
int i,j;
int k=0;
Matrix* temp;
temp=initMatrix(mat->column-1,mat->column-1);
for(i=0;i<mat->column;i++)
{
for(j=0;j<mat->column;j++)
{
if((i!=ro)&&(j!=co))
{
temp->data[k]=mat->data[i*mat->column+j];
k++;
}
}
}
return temp;
}
Matrix* getInverseMatrix(Matrix* mat)
{
int i,j;
float k;
Matrix* temp;
temp=initMatrix(mat->column,mat->row);
if(DeterminantValue(mat)!=0)
{
for(i=0;i<mat->column;i++)
{
for(j=0;j<mat->column;j++)
{
if((i+j)%2==0)
k=1;
else
k=-1;
temp->data[i*mat->column+j]=(k*DeterminantValue(getLeftMatrix(mat,j,i)))/DeterminantValue(mat);
}
}
return temp;
}
else
{
printf("ERROR:the Matrix has no inverse matrix!\n");
return NULL;
}
}
void KalmanFilter(Kalman* kal,Matrix* measure)
{
if(measure!=NULL)
{
valuechange(kal->measurement,measure);
valuechange(kal->preState,matMultiple(kal->A,kal->lastState));
valuechange(kal->P_pre,matPlus(kal->Q,matMultiple(kal->A,matMultiple(kal->lastP,matTranspose(kal->A)))));
valuechange(kal->Kg,matMultiple(matMultiple(kal->P_pre,matTranspose(kal->H)),getInverseMatrix(matPlus(kal->R,matMultiple(kal->H,matMultiple(kal->P_pre,matTranspose(kal->H)))))));
valuechange(kal->modifyState,matPlus(kal->preState,matMultiple(kal->Kg,matMinus(kal->measurement,matMultiple(kal->H,kal->preState)))));
valuechange(kal->P_modify,matMultiple(matMinus(kal->I,matMultiple(kal->Kg,kal->H)),kal->P_pre));
}
}
void KalmanReadyForNext(Kalman* kal)
{
valuechange(kal->lastP,kal->P_modify);
valuechange(kal->lastState,kal->modifyState);
}?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -