MatrixWrapper::ColumnVector NonLinearAnalyticConditionalGaussian_Ginac::ExpectedValueGet() const { MatrixWrapper::ColumnVector u_num (u_size); MatrixWrapper::ColumnVector x_num (x_size); MatrixWrapper::ColumnVector func_num(func_size); GiNaC::ex substitute (func_size); MatrixWrapper::ColumnVector expected(func_size); u_num = ConditionalArgumentGet(1); x_num = ConditionalArgumentGet(0); // use Mu of additive noise if (cond_size!=0) for (unsigned int i=0; i<u_size; i++) for (unsigned int j=0; j<cond_size; j++) if (u_sym[i] == cond_sym[j]) u_num(i+1) += (this->AdditiveNoiseMuGet())(j+1); // evaluate func for (unsigned int i=0; i<func_size; i++) { // temp variable to substitute in substitute = func_sym(i,0); // substitute all u_sym with u_num for (unsigned int j=0; j<u_size; j++) substitute = substitute.subs( u_sym[j]==u_num(j+1) ); // substitute all x_sym with x_num for (unsigned int j=0; j<x_size; j++) substitute = substitute.subs( x_sym[j]==x_num(j+1) ); // build matrix func_num func_num(i+1) = GiNaC::ex_to<GiNaC::numeric>( substitute.evalf() ).to_double(); } expected = func_num; if (cond_size==0) expected += AdditiveNoiseMuGet(); return expected; }
MatrixWrapper::ColumnVector nonLinearAnalyticConditionalGaussian::ExpectedValueGet() const { MatrixWrapper::ColumnVector state = ConditionalArgumentGet(0); MatrixWrapper::ColumnVector angVel = ConditionalArgumentGet(1); MatrixWrapper::Matrix identity(4,4); identity.toIdentity(); // TODO I need state dimension here instead of hardcoding 4 MatrixWrapper::Matrix tmpA(4,4); OmegaOperator(angVel, tmpA); tmpA = tmpA*(0.5*m_threadPeriod); // NOTE On 27/07/2015 I changed the sign before AdditiveNoiseMuGet() to a 'minus' according to Choukroun's derivation in Novel Methods for Attitude Determination using Vector Observations // NOTE I think this is wrong!! as what I want to add here is noise with 0 mean and covariance Q^w_k!!! MatrixWrapper::ColumnVector noise = AdditiveNoiseMuGet(); state = state + tmpA*state - noise; // Normalizing the quaternion MatrixWrapper::Quaternion tmpQuat(state); tmpQuat.normalize(); // The other methods of the library use inputs of type ColumnVector, therefore a copy of the quaternion is necessary MatrixWrapper::ColumnVector ret(tmpQuat); return ret; }