qrSolver.compute(OS.transpose());
m_q=qrSolver.householderQ();
m_r = qrSolver.matrixQR().triangularView<Upper>();

S_pre = m_r.block(0,0,n,n).transpose();

if (w_c0 < 0)
    internal::llt_inplace<float,Upper>::rankUpdate(S_pre,
                sqrt(-w_c0)*(sigmaPoints.col(0) - state_pre),
    -1);
else
    internal::llt_inplace<float,Upper>::rankUpdate(S_pre,
                sqrt(w_c0)*(sigmaPoints.col(0) - state_pre),
    1);