void residuo(lmx::Vector<double>& res_in, lmx::Vector<double>& q_in) {
// x_i^3 = lhs_i
if ( lhs.size() == 0 ) {
temp.resize(res_in.size());
lhs.resize(res_in.size());
for (int i=0; i<lhs.size(); ++i)
lhs.writeElement( std::pow(i+1.,6), i); // lhs_i = i^6
}
temp.multElem( q_in, q_in ); //(x_i)^2
res_in.multElem( temp, q_in ); //(x_i)^3
res_in -= lhs;
}
MyDiffSystem(lmx::Vector<double> q1_in, lmx::Vector<double> q2_in)
{
localQ1.resize(q1_in.size());
localQ1 = q1_in;
localQ2.resize(q2_in.size());
localQ2 = q2_in;
H.resize(3,3);
H(0,0) = 2.;
H(1,1) = 3.;
H(2,2) = 4.;
K.resize(2,2);
K(0,0) = 2.;
K(1,0) = 1.;
K(0,1) = 3.;
K(1,1) = 1.;
}
void jacobiano(lmx::Matrix<double>& jac_in, lmx::Vector<double>& q_in) {
for (int i=0; i<q_in.size() ; ++i)
jac_in.writeElement( 3.*q_in.readElement(i)*q_in.readElement(i), i, i);
}