ADHOC/replicant-frameworks_native
3
0
Fork 0
Code Issues 6 Pull Requests Releases Wiki Activity

improve sensor fusion

Browse Source 
we now use a better quaternion propagation equation
this is especially beneficial for lower gyroscope rates

Change-Id: Ifbf273c8a092a8849ca4fe4b9bca30787e924018
This commit is contained in:
Mathias Agopian 2012-06-28 18:21:43 -07:00
parent 8f11b24a72
commit bdf277355d
1 changed files with 23 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
services/sensorservice/Fusion.cpp
View File

@ -389,10 +389,18 @@ void Fusion::predict(const vec3_t& w, float dT) {
const vec4_t q = x0;
const vec3_t b = x1;
const vec3_t we = w - b;
const vec4_t dq = getF(q)*((0.5f*dT)*we);
x0 = normalize_quat(q + dq);
// q(k+1) = O(we)*q(k)
// --------------------
//
// O(w) = | cos(0.5*||w||*dT)*I33 - [psi]x psi |
// | -psi' cos(0.5*||w||*dT) |
//
// psi = sin(0.5*||w||*dT)*w / ||w||
//
//
// P(k+1) = Phi(k)*P(k)*Phi(k)' + G*Q(k)*G'
// ----------------------------------------
//
// G = | -I33 0 |
// | 0 I33 |
@ -413,13 +421,26 @@ void Fusion::predict(const vec3_t& w, float dT) {
const mat33_t wx(crossMatrix(we, 0));
const mat33_t wx2(wx*wx);
const float lwedT = length(we)*dT;
const float hlwedT = 0.5f*lwedT;
const float ilwe = 1/length(we);
const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
const float k1 = sinf(lwedT);
const float k2 = cosf(hlwedT);
const vec3_t psi(sinf(hlwedT)*ilwe*we);
const mat33_t O33(crossMatrix(-psi, k2));
mat44_t O;
O[0].xyz = O33[0]; O[0].w = -psi.x;
O[1].xyz = O33[1]; O[1].w = -psi.y;
O[2].xyz = O33[2]; O[2].w = -psi.z;
O[3].xyz = psi; O[3].w = k2;
Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0;
Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
x0 = O*q;
if (x0.w < 0)
x0 = -x0;
P = Phi*P*transpose(Phi) + GQGt;
checkState();