void gen(int kk, mat &a, vec &b)
{
const int m = kk+2;
b.assign(m, 1<<kk);
a.assign(m,vec(m,0));
a[0][0] = a[0][1] = 1;
for (int i = 1; i < m; i++) {
int kkk = kk-i+1;
for (int j = 0; j <= kkk; j++) a[i][j+1] = c[kkk][j];
}
}
/*
* This motion model is driven by IMU measurements and random perturbations, and defined by:
* The state vector, x = [p q v ab wb g] , of size 19.
*
* The transition equation is
* - x+ = move_func(x,u,n),
*
* with u = [am, wm] the IMU measurements (the control input)
* and n = [vi, ti, abi, wbi] the perturbation impulse.
*
* the transition equation f() is decomposed as:
* #if AVGSPEED
* - p+ = p + (v + v+)/2*dt
* #else
* - p+ = p + v*dt
* #endif
* - q+ = q**((wm - wb)*dt + ti) <-- ** : quaternion product ; ti : theta impulse
* - v+ = v + (R(q)*(am - ab) + g)*dt + vi <-- am and wm: IMU measurements ; vi : v impulse
* - ab+ = ab + abi <-- abi : random walk in acc bias with abi impulse perturbation
* - wb+ = wb + wbi <-- wbi : random walk of gyro bias with wbi impulse perturbation
* - g+ = g <-- g : gravity vector, constant but unknown
* -----------------------------------------------------------------------------
*
* The Jacobian XNEW_x is built with
* var | p q v ab wb g
* pos | 0 3 7 10 13 16
* -------+----------------------------------------------------
* #if AVGSPEED
* p 0 | I VNEW_q*dt/2 I*dt -R*dt*dt/2 0 I*dt*dt/2
* #else
* p 0 | I 0 I*dt 0 0 0
* #endif
* q 3 | 0 QNEW_q 0 0 QNEW_wb 0
* v 7 | 0 VNEW_q I -R*dt 0 I*dt
* ab 10 | 0 0 0 I 0 0
* wb 13 | 0 0 0 0 I 0
* g 16 | 0 0 0 0 0 I
* -----------------------------------------------------------------------------
*
* The Jacobian XNEW_pert is built with
* var | vi ti abi wbi
* pos | 0 3 6 9
* -------+----------------------
* #if AVGSPEED
* p 0 |I.dt/2 0 0 0
* #else
* p 0 | 0 0 0 0
* #endif
* q 3 | 0 QNEW_ti 0 0
* v 7 | I 0 0 0
* ab 10 | 0 0 I 0
* wb 13 | 0 0 0 I
* g 16 | 0 0 0 0
* -----------------------------------------------------------------------------
*/
void RobotInertial::move_func(const vec & _x, const vec & _u, const vec & _n, double _dt, vec & _xnew, mat & _XNEW_x,
mat & _XNEW_pert) {
// Separate things out to make it clearer
vec3 p, v, ab, wb, gv;
vec g;
vec4 q;
splitState(_x, p, q, v, ab, wb, g); // split state vector
if (g_size == 1) { gv = z_axis * g(0); } else { gv = g; }
// Split control and perturbation vectors into
// sensed acceleration and sensed angular rate
// and noises
vec3 am, wm, vi, ti, abi, wbi; // measurements and random walks
splitControl(_u, am, wm);
splitPert(_n, vi, ti, abi, wbi);
// It is useful to start obtaining a nice rotation matrix and the product R*dt
Rold = q2R(q);
Rdt = Rold * _dt;
// Invert sensor functions. Get true acc. and ang. rates
// a = R(q)(asens - ab) + g true acceleration
// w = wsens - wb true angular rate
vec3 atrue, wtrue;
atrue = prod(Rold, (am - ab)) + gv; // could have done rotate(q, instead of prod(Rold,
wtrue = wm - wb;
// Get new state vector
vec3 pnew, vnew, abnew, wbnew;
vec gnew;
vec4 qnew;
// qnew = q x q(w * dt)
// Keep qwt ( = q(w * dt)) for later use
vec4 qwdt = v2q(wtrue * _dt + ti);
qnew = qProd(q, qwdt); // orientation
vnew = v + atrue * _dt + vi; // velocity
#if AVGSPEED
pnew = p + (v+vnew)/2 * _dt; // position
#else
pnew = p + v * _dt; // position
#endif
abnew = ab + abi; // acc bias
wbnew = wb + wbi; // gyro bias
gnew = g; // gravity does not change
// normalize quaternion
ublasExtra::normalizeJac(qnew, QNORM_qnew);
ublasExtra::normalize(qnew);
// Put it all together - this is the output state
unsplitState(pnew, qnew, vnew, abnew, wbnew, gnew, _xnew);
// Now on to the Jacobian...
// Identity is a good place to start since overall structure is like this
// var | p q v ab wb g
// pos | 0 3 7 10 13 16
// -------+----------------------------------------------------
//#if AVGSPEED
// p 0 | I VNEW_q*dt/2 I*dt -R*dt*dt/2 0 I*dt*dt/2
//#else
// p 0 | I 0 I*dt 0 0 0
//#endif
// q 3 | 0 QNEW_q 0 0 QNEW_wb 0
// v 7 | 0 VNEW_q I -R*dt 0 I*dt
// ab 10 | 0 0 0 I 0 0
// wb 13 | 0 0 0 0 I 0
// g 16 | 0 0 0 0 0 I
_XNEW_x.assign(identity_mat(state.size()));
// Fill in XNEW_v: VNEW_g and PNEW_v = I * dt
identity_mat I(3);
Idt = I * _dt;
mat Iz; if (g_size == 1) { Iz.resize(3,1); Iz.clear(); Iz(2,0)=1; } else { Iz = I; }
mat Izdt = Iz * _dt;
subrange(_XNEW_x, 0, 3, 7, 10) = Idt;
#if AVGSPEED
subrange(_XNEW_x, 0, 3, 16, 16+g_size) = Izdt*_dt/2;
#endif
subrange(_XNEW_x, 7, 10, 16, 16+g_size) = Izdt;
// Fill in QNEW_q
// qnew = qold ** qwdt ( qnew = q1 ** q2 = qProd(q1, q2) in rtslam/quatTools.hpp )
qProd_by_dq1(qwdt, QNEW_q);
subrange(_XNEW_x, 3, 7, 3, 7) = prod(QNORM_qnew, QNEW_q);
// Fill in QNEW_wb
// QNEW_wb = QNEW_qwdt * QWDT_wdt * WDT_w * W_wb
// = QNEW_qwdt * QWDT_w * W_wb
// = QNEW_qwdt * QWDT_w * (-1)
qProd_by_dq2(q, QNEW_qwdt);
// Here we get the derivative of qwdt wrt wtrue, so we consider dt = 1 and call for the derivative of v2q() with v = w*dt
// v2q_by_dv(wtrue, QWDT_w);
v2q_by_dv(wtrue*_dt, QWDT_w); QWDT_w *= _dt;
QNEW_w = prod ( QNEW_qwdt, QWDT_w);
subrange(_XNEW_x, 3, 7, 13, 16) = -prod(QNORM_qnew,QNEW_w);
// Fill VNEW_q
// VNEW_q = d(R(q)*v) / dq
rotate_by_dq(q, am-ab, VNEW_q); VNEW_q *= _dt;
subrange(_XNEW_x, 7, 10, 3, 7) = VNEW_q;
#if AVGSPEED
subrange(_XNEW_x, 0, 3, 3, 7) = VNEW_q*_dt/2;
#endif
// Fill in VNEW_ab
subrange(_XNEW_x, 7, 10, 10, 13) = -Rdt;
#if AVGSPEED
subrange(_XNEW_x, 0, 3, 10, 13) = -Rdt*_dt/2;
#endif
// Now on to the perturbation Jacobian XNEW_pert
// Form of Jacobian XNEW_pert
// It is like this:
// var | vi ti abi wbi
// pos | 0 3 6 9
// -------+----------------------
//#if AVGSPEED
// p 0 |I.dt/2 0 0 0
//#else
// p 0 | 0 0 0 0
//#endif
// q 3 | 0 QNEW_ti 0 0
// v 7 | I 0 0 0
// ab 10 | 0 0 I 0
// wb 13 | 0 0 0 I
// g 16 | 0 0 0 0
// Fill in the easy bits first
_XNEW_pert.clear();
#if AVGSPEED
ublas::subrange(_XNEW_pert, 0, 3, 0, 3) = Idt/2;
#endif
ublas::subrange(_XNEW_pert, 7, 10, 0, 3) = I;
ublas::subrange(_XNEW_pert, 10, 13, 6, 9) = I;
ublas::subrange(_XNEW_pert, 13, 16, 9, 12) = I;
// Tricky bit is QNEW_ti = d(qnew)/d(ti)
// Here, ti is the integral of the perturbation, ti = integral_{tau=0}^dt (wn(t) * dtau),
// with: wn: the angular rate measurement noise
// dt: the integration period
// ti: the resulting angular impulse
// The integral of the dynamic equation is:
// q+ = q ** v2q((wm - wb)*dt + ti)
// We have: QNEW_ti = QNEW_w / dt
// with: QNEW_w computed before.
// The time dependence needs to be included in perturbation.P(), proportional to perturbation.dt:
// U = perturbation.P() = U_continuous_time * dt
// with: U_continuous_time expressed in ( rad / s / sqrt(s) )^2 = rad^2 / s^3 <-- yeah, it is confusing, but true.
// (Use perturbation.set_P_from_continuous() helper if necessary.)
//
subrange(_XNEW_pert, 3, 7, 3, 6) = prod (QNORM_qnew, QNEW_w) * (1 / _dt);
}