泰勒展开最小二乘法求解TDOA

#include <iostream.h> #include <math.h> #include <iomanip.h> #include <string.h> #include "stdio.h" #include <stdlib.h> #include "matrix.h" #include "mymatrix.h" #define BSN 4 float Q[BSN-1][BSN-1]={{1,0,0},{0,1,0},{0,0,1}}; float BS[BSN-2][BSN]={{0,100,0,100},{0,0,100,100}}; float iep[2][1]={{35},{50}}; float myiep[2][1]={{27},{53}}; /* Taylorforline 展开公式*/ void Taylorforline(int n) { int i =0,j=0,k=0; double R1=0.0,R[BSN-1]={0.0}; double **A1,**A2,**A3,**A4,**A5; double **ht,**h,**G,**Gt; double **delt; double **IQ,**Q,**QI; double meadist[1][BSN]={{0.0,0.0,0.0,0.0}},h0[1][BSN]={{0.0,0.0,0.0,0.0}}; h=zeros(1,BSN-1); G=zeros(BSN-1,2); Q=zeros(3,3); for(i=0;i<BSN;i++) { meadist[0][i]=sqrt(pow(iep[0][0]-BS[0][i],2)+pow(iep[1][0]-BS[1][i],2)); } for(i=0;i<BSN-1;i++) { h0[0][i]=meadist[0][i+1]-meadist[0][0]+i*0.5+0.5; } for (k=0;k<n;k++) { R1=sqrt(pow(iep[0][0],2)+pow(iep[1][0],2));//当前坐标到基站1（0,0）的距离 for (j=0;j<BSN-1;j++) { R[j]=sqrt(pow(iep[0][0]-BS[0][j+1],2) + pow(iep[1][0] - BS[1][j+1],2)); } h=zeros(1,BSN-1); for(i=0;i<BSN-1;i++) { h[0][i]=h0[0][i]-(R[i]-R1); } ht=MatrixInver(h,1,3);//h的转置矩阵ht G=zeros(BSN-1,2); for(i=0;i<BSN-1;i++) { G[i][0]=-iep[0][0]/R1-(BS[0][i+1]-iep[0][0])/R[i]; G[i][1]=-iep[1][0]/R1-(BS[1][i+1]-iep[1][0])/R[i]; } //求解增量 Q[0][0]=1;Q[0][1]=0;Q[0][2]=0; Q[1][0]=0;Q[1][1]=1;Q[1][2]=0; Q[2][0]=0;Q[2][1]=0;Q[2][2]=1; Gt= MatrixInver(G,3,2);//G的转置矩阵Gt QI=MatrixOpp(Q,3); A1=MatrixMulti(Gt,QI,2,3,3); A2=MatrixMulti(A1,G,2,3,2); A3=MatrixOpp(A2,2); A4=MatrixMulti(A3,Gt,2,2,2); A5=MatrixMulti(A4,QI,2,2,3); delt=zeros(2,1); delt=MatrixMulti(A5,ht,2,3,1); cout<<"Taylor偏差为:"<<'('<<delt[0][0]<<','<<delt[1][0]<<')'<<endl; } iep[0][0]=iep[0][0]+delt[0][0]; iep[1][0]=iep[1][0]+delt[1][0]; } /* myTaylorforline 展开公式*/ void myTaylorforline(int n) { int i =0,j=0,k=0; float R1=0.0,R[BSN-1]={0.0}; float A1[10][10],A2[10][10],A3[10][10],A4[10][10],A5[10][10]; float ht[10][10],h[10][10],G[10][10],Gt[10][10]; float delt[10][10]; float IQ[10][10],Q[10][10],QI[10][10]; float meadist[1][BSN]={{0.0,0.0,0.0,0.0}},h0[1][BSN]={{0.0,0.0,0.0,0.0}}; myzeros(1,BSN-1,h); myzeros(BSN-1,2,G); myzeros(3,3,Q); myzeros(10,10,A1); myzeros(10,10,A2); myzeros(10,10,A3); myzeros(10,10,A4); myzeros(10,10,A5); myzeros(10,10,ht); myzeros(10,10,h); myzeros(10,10,G); myzeros(10,10,Gt); myzeros(10,10,delt); myzeros(10,10,QI); myzeros(10,10,IQ); for(i=0;i<BSN;i++) { meadist[0][i]=sqrt(pow(iep[0][0]-BS[0][i],2)+pow(iep[1][0]-BS[1][i],2)); } for(i=0;i<BSN-1;i++) { h0[0][i]=meadist[0][i+1]-meadist[0][0]+i*0.5+0.5; } for (k=0;k<n;k++) { myzeros(10,10,A1); myzeros(10,10,A2); myzeros(10,10,A3); myzeros(10,10,A4); myzeros(10,10,A5); R1=sqrt(pow(iep[0][0],2)+pow(iep[1][0],2));//当前坐标到基站1（0,0）的距离 for (j=0;j<BSN-1;j++) { R[j]=sqrt(pow(iep[0][0]-BS[0][j+1],2) + pow(iep[1][0] - BS[1][j+1],2)); } myzeros(1,BSN-1,h); for(i=0;i<BSN-1;i++) { h[0][i]=h0[0][i]-(R[i]-R1); } //ht=MatrixInver(h,1,3);//h的转置矩阵ht //myMatrixInver(int m,int n,float a[5][5],float b[5][5]) myMatrixInver(1,3,h,ht); myzeros(BSN-1,2,G); for(i=0;i<BSN-1;i++) { G[i][0]=-iep[0][0]/R1-(BS[0][i+1]-iep[0][0])/R[i]; G[i][1]=-iep[1][0]/R1-(BS[1][i+1]-iep[1][0])/R[i]; } //求解增量 Q[0][0]=1;Q[0][1]=0;Q[0][2]=0; Q[1][0]=0;Q[1][1]=1;Q[1][2]=0; Q[2][0]=0;Q[2][1]=0;Q[2][2]=1; //Gt= MatrixInver(G,3,2);//G的转置矩阵Gt //QI=MatrixOpp(Q,3); //A1=MatrixMulti(Gt,QI,2,3,3); //A2=MatrixMulti(A1,G,2,3,2); //A3=MatrixOpp(A2,2); //A4=MatrixMulti(A3,Gt,2,2,2); //A5=MatrixMulti(A4,QI,2,2,3); //delt=MatrixMulti(A5,ht,2,3,1); myMatrixInver(3,2,G,Gt); myMatrixOpp(3,Q,QI); myMatrixMulti(Gt,QI,A1,2,3,3); myMatrixMulti(A1,G,A2,2,3,2); myMatrixOpp(2,A2,A3); myMatrixMulti(A3,Gt,A4,2,2,2); myMatrixMulti(A4,QI,A5,2,2,3); myzeros(2,1,delt); myMatrixMulti(A5,ht,delt,2,3,1); cout<<"myTaylor偏差为:"<<'('<<delt[0][0]<<','<<delt[1][0]<<')'<<endl; } myiep[0][0]=myiep[0][0]+delt[0][0]; myiep[1][0]=myiep[1][0]+delt[1][0]; } void main() { float IQ[5][5],Q[5][5],QI[5][5]; int i,j; float g[5][5]={{0,0,0,0,0},{0,0,0,0,0},{0,0,0,0,0},{0,0,0,0,0},{0,0,0,0,0}},f[5][5],m[5][5]; myTaylorforline(5); for(i=0;i<1;i++) { cout<<"myTaylor估算坐标为:"<<'('<<myiep[0][0]<<','<<myiep[1][0]<<')'<<endl; printf("\n"); } Taylorforline(5); for(i=0;i<1;i++) { cout<<"Taylorfor估算坐标为:"<<'('<<iep[0][0]<<','<<iep[1][0]<<')'<<endl; } system("pause"); }