void MatterFreeE::Sound(double *S,double *Dencity,double *Energy,int Num)
{
VecCl T(Num),Eplus(Num),Tplus(Num),Dplus(Num),Pplus(Num),P(Num);
Temperature(T.Ptr,Dencity,Energy,Num);
FreeEPtr->Pressure(P.Ptr,Dencity,T.Ptr,Num);
Tplus= T * (1 + MinimT2EstpCoef) + MinimT2EstpMin;
FreeEPtr->Energy(Eplus.Ptr,Dencity,Tplus.Ptr,Num);
for (int k=1;k<=Num;k++)
{
Dplus[k]=Dencity[k];
if (Dencity[k]<MathZer) { cout<<"MatterFreeEIO::Sound; Dencity<MathZer :"
<<Dencity<<"\n";Tplus[k]=0;continue;}
double dE=max<double>(Eplus[k]-Energy[k],MathZer);
double dr=Dencity[k]+dE*sqr(Dencity[k])/max<double>(1e-4,P.Ptr[k]);
if (fabs(dr)<MathZer) {Tplus[k]=0;continue;}
Dplus[k]=Dplus[k]+dr;
}
FreeEPtr->Pressure(Pplus.Ptr,Dplus.Ptr,Tplus.Ptr,Num);
for (int k=1;k<=Num;k++) S[k]=sqrt( max<double>(0,Pplus[k]-P[k])/(Dplus[k]-Dencity[k]) );
};
void FifthCKF::FifthCKFfiltering(double *z, UINT m_length)
{
CMatrix Pplus(4, 4);
Pplus = glb.Pplus;
CMatrix xhat(4, 1);
xhat = glb.xhat;
StoreData *fifthckf = new StoreData(xhat(1, 1), xhat(2, 1), xhat(3, 1), xhat(4, 1));
X.Add(fifthckf);
CMatrix Shat(dim, dim);
CMatrix rjpoint1(dim, 1);
CMatrix rjpoint2(dim, cpoints);
CMatrix rjpoint3(dim, hcpoints - cpoints - 1);
CMatrix Xminus1(dim, 1);
CMatrix Xminus2(dim, cpoints);
CMatrix Repmat2(dim, cpoints);
CMatrix Xminus3(dim, hcpoints - cpoints - 1);
CMatrix Repmat3(dim, hcpoints - cpoints - 1);
CMatrix jtemp(dim, 1);
CMatrix Zhat1(1, 1);
CMatrix Zhat2(1, cpoints);
CMatrix RepmatZ2(1, cpoints);
CMatrix Zhat3(1, hcpoints - cpoints - 1);
CMatrix RepmatZ3(1, hcpoints - cpoints - 1);
CMatrix W(dim, 1);
CMatrix Pz(1, 1);
CMatrix Xtemp(dim, 1);
CMatrix Pxz(4, 1);
int num = 1;//第0个未知存放的是占位符0，并不是真正的测量值，因此应当从下标1开始取测量值
for (UINT count = TimeInterval; count <= m_length; count += TimeInterval)
{
//Time Update
//Evaluate the Cholesky factor
Shat = Pplus.Cholesky();
//Evaluate the cubature points and the propagated cubature points//Estimate the predicted state
CMatrix CX(4, 1);
for (int i = 1; i <= hcpoints; ++i)
{
for (int j = 1; j <= dim; ++j)
{
jtemp(j, 1) = glb.kesi_FifthCKF(j, i);
}
jtemp = Shat * jtemp + xhat;
Xtemp = glb.fai * jtemp;
if ( 1 == i)
{
for (int j = 1; j <= dim; ++j)
{
Xminus1(j, 1) = Xtemp(j, 1);
}
CX = CX + c5_w2 * Xtemp;
}
else if (i > 1 && i <= cpoints + 1)//此编程风格会提高代码的冗余度，但提高了程序的运行效率，下同
{
for (int j = 1; j <= dim; ++j)
{
Xminus2(j, i - 1) = Xtemp(j, 1);
}
CX = CX + c5_w1 * Xtemp;
}
else
{
for (int j = 1; j <= dim; ++j)
{
Xminus3(j, i - cpoints - 1) = Xtemp(j, 1);
}
CX = CX + c5_w4 * Xtemp;
}
}
//xhat = CX;
//Estimate the predicted error covariance
for (int i = 1; i <= cpoints; ++i)
{
#pragma omp parallel for
for (int j = 1; j <= dim; ++j)
{
Repmat2(j, i) = CX(j, 1);
}
}
for (int i = 1; i <= hcpoints - cpoints - 1; ++i)
{
#pragma omp parallel for
for(int j = 1; j <= dim; ++j)
{
Repmat3(j, i) = CX(j, 1);
}
}
Pplus = c5_w2 * ((Xminus1 - CX) * (~(Xminus1 - CX))) +
c5_w1 * (Xminus2 - Repmat2) * (~(Xminus2 - Repmat2)) +
c5_w4 * (Xminus3 - Repmat3) * (~(Xminus3 - Repmat3)) + glb.gama * glb.Im * (~glb.gama);
//Measurement Update
//Evaluate the Cholesky factor
Shat = Pplus.Cholesky();
//Evaluate the cubature points and the propagated cubature points//Estimate the predicted measurement
CMatrix Z(1, 1);
for (int i = 1; i <= hcpoints; ++i)
{
for (int j = 1; j <= dim; ++j)
{
jtemp(j, 1) = glb.kesi_FifthCKF(j, i);
}
jtemp = Shat * jtemp + CX;
if (1 == i)
{
Zhat1(1, 1) = atan(jtemp(3, 1) / jtemp(1, 1));
Z(1, 1) = Z(1, 1) + c5_w2 * Zhat1(1, 1);
#pragma omp parallel for
for (int j = 1; j<= dim; ++j)
{
rjpoint1(j, 1) = jtemp(j, 1);
}
}
else if (i > 1 && i <= cpoints + 1)
{
Zhat2(1, i - 1) = atan(jtemp(3, 1) / jtemp(1, 1));
Z(1, 1) = Z(1, 1) + c5_w1 * Zhat2(1, i - 1);
#pragma omp parallel for
for (int j = 1; j <= dim; ++j)
{
rjpoint2(j, i - 1) = jtemp(j, 1);
}
}
else
{
Zhat3(1, i - cpoints - 1) = atan(jtemp(3, 1) / jtemp(1, 1));
Z(1, 1) = Z(1, 1) + c5_w4 * Zhat3(1, i - cpoints - 1);
#pragma omp parallel for
for (int j = 1; j <= dim; ++j)
{
rjpoint3(j, i - cpoints - 1) = jtemp(j, 1);
}
}
}
//Estimate the innovation covariance matrix
Pz(1, 1) = Rn;//For saving memory
#pragma omp parallel for
for (int i = 1; i <= cpoints; ++i)
{
RepmatZ2(1, i) = Z(1, 1);
}
#pragma omp parallel for
for (int i = 1; i <= hcpoints - cpoints - 1; ++i)
{
RepmatZ3(1, i) = Z(1, 1);
}
Pz = c5_w2 * (Zhat1 - Z) * (~(Zhat1 - Z)) +
c5_w1 * (Zhat2 - RepmatZ2)* (~(Zhat2 - RepmatZ2)) +
c5_w4 * (Zhat3 - RepmatZ3) * (~(Zhat3 - RepmatZ3)) + Pz;
//Estimate the cross-covariance matrix
Pxz = c5_w2 * (rjpoint1 - CX) * (~(Zhat1 - Z)) +
c5_w1 *(rjpoint2 - Repmat2) * (~(Zhat2 - RepmatZ2)) +
c5_w4 * (rjpoint3 - Repmat3) * (~(Zhat3 - RepmatZ3));
//Estimate the Kalman gain
W = ((double)1 / Pz(1, 1)) * Pxz;
//Estimate the updated state
//Znum(1, 1) = z[num];
xhat = CX + W * (z[num] - Z(1, 1));
++num;
Pplus = Pplus - Pz(1, 1) * W * (~W);
StoreData *fifthckf = new StoreData(xhat(1, 1), xhat(2, 1), xhat(3, 1), xhat(4, 1));
X.Add(fifthckf);
}
}
