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

revert parts of dc5b63e40, which made gyro drift estimation unstable

Browse Source 
initialize the system covariance matrix with non-zero
    values for the gyro-bias part. this improves the initial
    bias estimation speed significantly.

the initial covariance matrix should be small because the drift
changes slowly. the real problem is that we're not starting with
a good estimate of the drift, which this algorithm relies on.

so with this revert, it'll take a while for the drift to be estimated
but it won't be unstable.

Change-Id: Id5584bc114a2390d507643b2451b2650c1b90721
This commit is contained in:
Mathias Agopian 2013-05-30 14:26:36 -07:00
parent 2e2a560c4b
commit 24f9bea8eb
1 changed files with 0 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
services/sensorservice/Fusion.cpp
View File

@ -220,22 +220,6 @@ void Fusion::initFusion(const vec4_t& q, float dT)
// initial covariance: Var{ x(t0) } // initial covariance: Var{ x(t0) }
// TODO: initialize P correctly // TODO: initialize P correctly
P = 0; P = 0;
// it is unclear how to set the initial covariance. It does affect
// how quickly the fusion converges. Experimentally it would take
// about 10 seconds at 200 Hz to estimate the gyro-drift with an
// initial covariance of 0, and about a second with an initial covariance
// of about 1 deg/s.
const float covv = 0;
const float covu = 0.5f * (float(M_PI) / 180);
mat33_t& Pv = P[0][0];
Pv[0][0] = covv;
Pv[1][1] = covv;
Pv[2][2] = covv;
mat33_t& Pu = P[1][1];
Pu[0][0] = covu;
Pu[1][1] = covu;
Pu[2][2] = covu;
} }
bool Fusion::hasEstimate() const { bool Fusion::hasEstimate() const {