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