bool SRUKF::measurementUpdate(const Matrix& measurement, const Matrix& measurementNoise, const Matrix& predictedMeasurementSigmas, const Matrix& stateEstimateSigmas)
{
int numberOfSigmaPoints = stateEstimateSigmas.getn();
debug << "Predicted measurement sigmas:" << std::endl << predictedMeasurementSigmas;
Matrix predictedMeasurement = CalculateMeanFromSigmas(predictedMeasurementSigmas);
Matrix Mz(m_numStates,numberOfSigmaPoints, false);
Matrix Mx(m_numStates,numberOfSigmaPoints, false);
for(int i = 0; i < numberOfSigmaPoints; i++)
{
Mz.setCol(i, m_sqrtSigmaWeights[0][i] * (predictedMeasurementSigmas.getCol(i) - predictedMeasurement));
Mx.setCol(i, m_sqrtSigmaWeights[0][i] * (stateEstimateSigmas.getCol(i) - m_mean));
}
Matrix Sz = horzcat(Mz,measurementNoise);
Matrix Pxz = Mx*Mz.transp();
Matrix K = Pxz * Invert22(Sz*Sz.transp());
debug << "K:" << std::endl << K;
m_mean = m_mean + K * (measurement - predictedMeasurement);
m_sqrtCovariance = HT(horzcat(Mx-K*Mz,K*measurementNoise));
//m_covariance = HT(horzcat(sigmaPoints-m_mean*m_sigmaWeights - K*predictedObservationSigmas +
// K*predictedObservation*m_sigmaWeights,K*measurementNoise));
return true;
}
void srTCSR::computeOffAxisTrajArrays(srTFieldBasedArrays& FldArr, srTMagFldCont* pMagLensCont)
{
if(pMagLensCont == 0) return;
double *pX1p = FldArr.X1pArr, *pZ1p = FldArr.Z1pArr;
double *pX2p = FldArr.X2pArr, *pZ2p = FldArr.Z2pArr;
double *pX1 = FldArr.X1Arr, *pZ1 = FldArr.Z1Arr;
double *pX2 = FldArr.X2Arr, *pZ2 = FldArr.Z2Arr;
double s = FldArr.sStart;
double sStp = FldArr.sStep;
for(int i=0; i<FldArr.Ns; i++)
{
TMatrix2d Mx(1,0,1,0), Mz(1,0,1,0);
pMagLensCont->ComputeParticlePropagMatrix(s, Mx, Mz);
*(pX1++) = Mx.Str0.x;
*(pX2++) = Mx.Str0.y;
*(pX1p++) = Mx.Str1.x;
*(pX2p++) = Mx.Str1.y;
*(pZ1++) = Mz.Str0.x;
*(pZ2++) = Mz.Str0.y;
*(pZ1p++) = Mz.Str1.x;
*(pZ2p++) = Mz.Str1.y;
s += sStp;
}
}