📄 lambda.cpp
字号:
#include "stdafx.h"
//#include "windows.h"
#include "Lambda.h"
#include "fstream.h"
#include "iomanip.h"
#include "math.h"
#include "Lambda.h"
#include "MatrixLYQ.h"
//#include "GlobleDefine.h"
int LAMBDA(MATRIX Qahat, MATRIX aFloat, int nCands, MATRIX & Afixed, MATRIX & SQahatnorm)
{
MATRIX Incur(aFloat.nRow,1);
MATRIX QahatZ(Qahat.nRow,Qahat.nCol);
MATRIX Z(Qahat.nRow,Qahat.nCol);
MATRIX L(Qahat.nRow,Qahat.nCol);
MATRIX D(Qahat.nRow,Qahat.nCol);
MATRIX Zt(Qahat.nRow,Qahat.nCol);
double dChi2;
for(int i = 0;i < aFloat.nRow;i ++)
{
for(int j = 0;j < aFloat.nCol;j ++)
{
Incur.pMATRIX[i][j] = int(aFloat.pMATRIX[i][j]);
aFloat.pMATRIX[i][j] = aFloat.pMATRIX[i][j] - Incur.pMATRIX[i][j];
}
}
Decorrel(Qahat,aFloat,QahatZ,Z,L,D,Zt); // LtDL-Decomposition
dChi2 = ChiStart(D, L,Zt,nCands); // 找出搜索半径
lSearch(Zt,L, D,dChi2,nCands,Afixed,SQahatnorm); // 固定出最终的整周模糊度
Afixed = !(!Afixed * (~Z));
//CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
Incur = RepmatMatrix(Incur,1,nCands);
//protend nCands is eQahatual to 1 ,and negnect this step firstly
for(i = 0;i < Afixed.nRow;i ++)
{
for(int j = 0;j < Afixed.nCol;j ++)
{
Afixed.pMATRIX[i][j] = Afixed.pMATRIX[i][j] + Incur.pMATRIX[i][j];
if (Afixed.pMATRIX[i][j] > 0)
{
Afixed.pMATRIX[i][j] = int(Afixed.pMATRIX[i][j] + 0.5);
}
else
{
Afixed.pMATRIX[i][j] = int(Afixed.pMATRIX[i][j] - 0.5);
}
}
}
ofstream out;
out.open("Fixed Ambiguty.txt",ios::out,filebuf::openprot);
out<<"Fixed Ambiguity :"<<endl;
out<<setiosflags(ios::left);
for(i = 0;i < Afixed.nRow;i ++)
{
out<<" ";
for(int j = 0;j < Afixed.nCol;j ++)
{
out<<setw(20)<<setprecision(10);
out<<Afixed.pMATRIX[i][j];
}
out<<endl;
}
out<<"Qahat.norm :"<<endl;
out<<setiosflags(ios::left)<<setw(20)<<setprecision(10);
for(i = 0;i < SQahatnorm.nRow;i ++)
{
out<<" ";
for(int j = 0;j < SQahatnorm.nCol;j ++)
{
out<<setw(20)<<setprecision(10);
out<<SQahatnorm.pMATRIX[i][j];
}
out<<endl;
}
// AfxMessageBox("LAMBDA算法搜索整周模糊度完成,结果保存在Fixed Ambiguty.txt文件中");
return 1;
}
// **************************************************************************
int Decorrel(MATRIX Qahat, MATRIX aFloat, MATRIX & QahatZ, MATRIX & Z, MATRIX & L, MATRIX & D, MATRIX & Za)
{
int nn;
nn = Qahat.nRow; //Qahat矩阵的行数n = size(Qahat,1);
MATRIX Zti(nn,nn); //
int ni1;
double dMu;
double dMid;
int nSw;
int ni;
for(int i = 0; i < Zti.nRow;i ++)//Zti = eye(n);
{
Zti.pMATRIX[i][i] = 1;
}
// nn = nn - 1; //由于matlab与C 中数组的起始下标不同
ni1 = nn - 1; //i1 = n -1,
nSw = 1; //只是一个中间变量
LtDLComposition(Qahat,L,D);//LtDL-decomposition
while (nSw != 0)
{
ni = Qahat.nRow - 1;//i = n;
nSw = 0;
while ((!nSw) && (ni > 0))//while (~sw) & (i > 1)
{
ni = ni - 1;//i = i -1;
if (ni < ni1)//if(i <= i1)
{
for(int j = ni + 1; j < nn ; j ++)//for j = i + 1:n
{
if (L.pMATRIX[j][ni] > 0)//mu = round(L(j,n))
{
dMu = (int)(L.pMATRIX[j][ni] + 0.5);
}
else
{
dMu = (int)(L.pMATRIX[j][ni] - 0.5);
}
if (dMu != 0)//if mu ~= 0
{
for(int nj = j; nj < nn;nj ++)//L(j:n,i) = L(j:n,i) - mu * L(j:n,j)
{
L.pMATRIX[nj][ni] = L.pMATRIX[nj][ni] - dMu * L.pMATRIX[nj][j];
}
for(int t = 0;t < nn; t ++)//Zti(1:n,j) = Zti(1:n,j) + mu * Zti(1:n,i);
{
Zti.pMATRIX[t][j] = Zti.pMATRIX[t][j] + dMu * Zti.pMATRIX[t][ni];
}
}
}
}
double dDelta;
//delta = D(i) + L(i+1,i)^2 * D(i + 1)
dDelta = D.pMATRIX[ni][ni] + pow(L.pMATRIX[ni + 1][ni],2) * D.pMATRIX[ni + 1][ni + 1];
if (dDelta < D.pMATRIX[ni + 1][ni + 1]) //if(delta < D(i + 1))
{
dMid = D.pMATRIX[ni + 1][ni + 1] * L.pMATRIX[ni + 1][ni] / dDelta;//lambda(3) = D(i + 1) * L(i + 1,i) / delta;
double dEta;
dEta = D.pMATRIX[ni][ni] / dDelta; //eta = D(i) / delta;
D.pMATRIX[ni][ni] = dEta * D.pMATRIX[ni + 1][ni + 1];//D(i) = eta * D(i+1);
D.pMATRIX[ni + 1][ni + 1] = dDelta;//D(i+1) = delta;
MATRIX Help(ni,1);
for(int u = 0;u < ni; u ++)
{
Help.pMATRIX[u][0] = L.pMATRIX[ni + 1][u] - L.pMATRIX[ni + 1][ni] * L.pMATRIX[ni][u];// Help = L(i+1,1:i-1) - L(i+1,i) .* L(i,1:i-1);
L.pMATRIX[ni + 1][u] = dMid * L.pMATRIX[ni + 1][u] + dEta * L.pMATRIX[ni][u];// L(i+1,1:i-1) = lambda(3) * L(i+1,1:i-1) + eta * L(i,1:i-1);
L.pMATRIX[ni][u] = Help.pMATRIX[u][0];//L(i,1:i-1) = Help;
}
L.pMATRIX[ni + 1][ni] = dMid;//L(i+1,i) = lambda(3);
MATRIX Help1(nn - ni-2,1);//
for(int v = ni + 2,m = 0;v < nn;v++,m ++)//
{
Help1.pMATRIX[m][0] = L.pMATRIX[v][ni];// Help = L(i+2:n,i);
L.pMATRIX[v][ni] = L.pMATRIX[v][ni + 1];//L(i+2:n,i) = L(i+2:n,i+1);
L.pMATRIX[v][ni + 1] = Help1.pMATRIX[m][0];//L(i+2:n,i+1) = Help;
}
MATRIX Help2(nn,1);
for(u= 0;u < nn;u ++)
{
Help2.pMATRIX[u][0] = Zti.pMATRIX[u][ni];//Help = Zti(1:n,i);
Zti.pMATRIX[u][ni] = Zti.pMATRIX[u][ni + 1];//Zti(1:n,i) = Zti(1:n,i+1);
Zti.pMATRIX[u][ni + 1] = Help2.pMATRIX[u][0];//Zti(1:n,i+1) = Help;
}
ni1 = ni + 1;//i1 = i;
nSw = 1; //sw = 1;
}
}
}
Z = ~(!Zti);//Z = inv(Zti');//转换矩阵
for(i = 0;i < Z.nRow;i ++)
{
for(int j = 0;j < Z.nCol;j ++)
{
if (Z.pMATRIX[i][j] > 0)
{
Z.pMATRIX[i][j] = int(Z.pMATRIX[i][j] + 0.5);
}
else
{
Z.pMATRIX[i][j] = int(Z.pMATRIX[i][j] - 0.5);
}
}
}
QahatZ = !Z * Qahat * Z; //Qahat = Z' * Qahat * Z;//转换以后的方差－协方差阵
Za = !Z * aFloat; //变换以后的整周模糊度矩阵
return 1;
}
// ******************************************************************************
// ***************************************************************************
double ChiStart(MATRIX D, MATRIX L, MATRIX Afloat, int nCands)
{
int nArgin;
double dFactor;
double dChi2;
MATRIX Linv;
MATRIX Dinv;
MATRIX dist(Afloat.nRow,Afloat.nCol);
MATRIX Ee;
double dMid;
double dSortMid;
int nTemp;
nArgin = 8;
dFactor = 0;
if (nArgin < 4)
{
nCands = 1;
}
if (nArgin < 5)
{
dFactor = 1.5;
}
int nN;
if (Afloat.nRow >= Afloat.nCol)
{
nN = Afloat.nRow;
}
else
{
nN = Afloat.nCol;
}
if (nCands == 1)
{
MATRIX Afixed;
MATRIX AfloatH;
double dW;
Afixed = Afloat;
AfloatH = Afloat;
for(int i=0;i < nN;i ++)
{
dW = 0;
for(int j =0;j < nN - 1; j ++)
{
dW += L.pMATRIX[i][j] * (Afloat.pMATRIX[j][0] - Afixed.pMATRIX[j][0]);
}
Afloat.pMATRIX[i][0] = Afloat.pMATRIX[i][0] - dW;
if(Afloat.pMATRIX[i][0] > 0)
{
Afixed.pMATRIX[i][0] = int(Afloat.pMATRIX[i][0] + 0.5);
}
else
{
Afixed.pMATRIX[i][0] = int(Afloat.pMATRIX[i][0] - 0.5);
}
}
MATRIX Temp(Afixed.nRow,Afixed.nCol);
for(i = 0;i < Afixed.nRow;i ++)
{
for(int j = 0; j < Afixed.nCol; j ++)
{
Temp.pMATRIX[i][j] = AfloatH.pMATRIX[i][j] - Afixed.pMATRIX[i][j];
}
}
Temp = !Temp * (~(!L * D * L)) * Temp;
dChi2 = Temp.pMATRIX[0][0] + 1E-6;
}
else if (nCands <= nN + 1)
{
Linv = ~L;
Dinv = ~D;
for(int i = 0;i < Afloat.nRow;i ++)
{//取小数部分
for(int j = 0;j < Afloat.nCol;j ++)
{
if (Afloat.pMATRIX[i][j] > 0)
{
dMid = int(Afloat.pMATRIX[i][j] + 0.5);
}
else
{
dMid = int(Afloat.pMATRIX[i][j] - 0.5);
}
dist.pMATRIX[i][j] = dMid - Afloat.pMATRIX[i][j];
}
}
Ee = (!Linv) * dist;
MATRIX Chi(1,nN + 1);
double dSum;
MATRIX SUM(1,1);
SUM = !(Dinv * Ee) * Ee;
Chi.pMATRIX[0][nN] = SUM.pMATRIX[0][0];
for( i = 0; i < nN; i ++)
{
Chi.pMATRIX[0][i] = 0;
for(int j = 0;j <= i; j ++)
{
Chi.pMATRIX[0][i] = Chi.pMATRIX[0][i] +
Dinv.pMATRIX[j][j] * Linv.pMATRIX[i][j] *
(2 * Ee.pMATRIX[j][0] + Linv.pMATRIX[i][j]);
}
Chi.pMATRIX[0][i] = Chi.pMATRIX[0][nN] + fabs(Chi.pMATRIX[0][i]);
}
for(i = 0;i < Chi.nCol;i ++)
{
nTemp = i;
for(int j = i + 1;j < Chi.nCol; j ++)
{
if(Chi.pMATRIX[0][j] < Chi.pMATRIX[0][nTemp])
{
nTemp = j;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -