Ejemplos de Mz en C++ (Cpp)

Ejemplo n.º 1

Archivo: SRUKF.cpp Proyecto: shannonfenn/Multi-Platform-Vision-System

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;
}

Ejemplo n.º 2

Archivo: srcradint.cpp Proyecto: erikjasiak/SRW

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;
    }
}