ADHOC/replicant-frameworks_native
3
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
010e422301
replicant-frameworks_native/services/sensorservice/Fusion.cpp
Mathias Agopian 3301542828 use quaternions instead of MRPs 
also use correct time propagation equation
disable the fused sensors when gyro is not present since
they were unusable in practice.

Change-Id: Iad797425784e67dc6c5690e97c71c583418cc5b5
2012-06-27 17:07:55 -07:00

345 lines
8.4 KiB
C++
Raw Blame History

/*
* Copyright (C) 2011 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <stdio.h>
#include <utils/Log.h>
#include "Fusion.h"
namespace android {
// -----------------------------------------------------------------------
static const float gyroSTDEV = 3.16e-4; // rad/s^3/2
static const float accSTDEV = 0.05f; // m/s^2 (measured 0.08 / CDD 0.05)
static const float magSTDEV = 0.5f; // uT (measured 0.7 / CDD 0.5)
static const float biasSTDEV = 3.16e-5; // rad/s^1/2 (guessed)
static const float FREE_FALL_THRESHOLD = 0.981f;
// -----------------------------------------------------------------------
template <typename TYPE, size_t C, size_t R>
static mat<TYPE, R, R> scaleCovariance(
const mat<TYPE, C, R>& A,
const mat<TYPE, C, C>& P) {
// A*P*transpose(A);
mat<TYPE, R, R> APAt;
for (size_t r=0 ; r<R ; r++) {
for (size_t j=r ; j<R ; j++) {
double apat(0);
for (size_t c=0 ; c<C ; c++) {
double v(A[c][r]*P[c][c]*0.5);
for (size_t k=c+1 ; k<C ; k++)
v += A[k][r] * P[c][k];
apat += 2 * v * A[c][j];
}
APAt[j][r] = apat;
APAt[r][j] = apat;
}
}
return APAt;
}
template <typename TYPE, typename OTHER_TYPE>
static mat<TYPE, 3, 3> crossMatrix(const vec<TYPE, 3>& p, OTHER_TYPE diag) {
mat<TYPE, 3, 3> r;
r[0][0] = diag;
r[1][1] = diag;
r[2][2] = diag;
r[0][1] = p.z;
r[1][0] =-p.z;
r[0][2] =-p.y;
r[2][0] = p.y;
r[1][2] = p.x;
r[2][1] =-p.x;
return r;
}
template<typename TYPE, size_t SIZE>
class Covariance {
mat<TYPE, SIZE, SIZE> mSumXX;
vec<TYPE, SIZE> mSumX;
size_t mN;
public:
Covariance() : mSumXX(0.0f), mSumX(0.0f), mN(0) { }
void update(const vec<TYPE, SIZE>& x) {
mSumXX += x*transpose(x);
mSumX += x;
mN++;
}
mat<TYPE, SIZE, SIZE> operator()() const {
const float N = 1.0f / mN;
return mSumXX*N - (mSumX*transpose(mSumX))*(N*N);
}
void reset() {
mN = 0;
mSumXX = 0;
mSumX = 0;
}
size_t getCount() const {
return mN;
}
};
// -----------------------------------------------------------------------
Fusion::Fusion() {
Phi[0][1] = 0;
Phi[1][1] = 1;
Ba.x = 0;
Ba.y = 0;
Ba.z = 1;
Bm.x = 0;
Bm.y = 1;
Bm.z = 0;
init();
}
void Fusion::init() {
mInitState = 0;
mGyroRate = 0;
mCount[0] = 0;
mCount[1] = 0;
mCount[2] = 0;
mData = 0;
}
void Fusion::initFusion(const vec4_t& q, float dT)
{
// initial estimate: E{ x(t0) }
x0 = q;
x1 = 0;
// process noise covariance matrix
// G = | -1 0 |
// | 0 1 |
const float v = gyroSTDEV;
const float u = biasSTDEV;
const float q00 = v*v*dT + 0.33333f*(dT*dT*dT)*u*u;
const float q10 = 0.5f*(dT*dT) *u*u;
const float q01 = q10;
const float q11 = u*u*dT;
GQGt[0][0] = q00;
GQGt[1][0] = -q10;
GQGt[0][1] = -q01;
GQGt[1][1] = q11;
// initial covariance: Var{ x(t0) }
P = 0;
}
bool Fusion::hasEstimate() const {
return (mInitState == (MAG|ACC|GYRO));
}
bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
if (hasEstimate())
return true;
if (what == ACC) {
mData[0] += d * (1/length(d));
mCount[0]++;
mInitState |= ACC;
} else if (what == MAG) {
mData[1] += d * (1/length(d));
mCount[1]++;
mInitState |= MAG;
} else if (what == GYRO) {
mGyroRate = dT;
mData[2] += d*dT;
mCount[2]++;
if (mCount[2] == 64) {
// 64 samples is good enough to estimate the gyro drift and
// doesn't take too much time.
mInitState |= GYRO;
}
}
if (mInitState == (MAG|ACC|GYRO)) {
// Average all the values we collected so far
mData[0] *= 1.0f/mCount[0];
mData[1] *= 1.0f/mCount[1];
mData[2] *= 1.0f/mCount[2];
// calculate the MRPs from the data collection, this gives us
// a rough estimate of our initial state
mat33_t R;
vec3_t up(mData[0]);
vec3_t east(cross_product(mData[1], up));
east *= 1/length(east);
vec3_t north(cross_product(up, east));
R << east << north << up;
const vec4_t q = matrixToQuat(R);
initFusion(q, mGyroRate);
}
return false;
}
void Fusion::handleGyro(const vec3_t& w, float dT) {
if (!checkInitComplete(GYRO, w, dT))
return;
predict(w, dT);
}
status_t Fusion::handleAcc(const vec3_t& a) {
// ignore acceleration data if we're close to free-fall
if (length(a) < FREE_FALL_THRESHOLD)
return BAD_VALUE;
if (!checkInitComplete(ACC, a))
return BAD_VALUE;
const float l = 1/length(a);
update(a*l, Ba, accSTDEV*l);
return NO_ERROR;
}
status_t Fusion::handleMag(const vec3_t& m) {
// the geomagnetic-field should be between 30uT and 60uT
// reject obviously wrong magnetic-fields
if (length(m) > 100)
return BAD_VALUE;
if (!checkInitComplete(MAG, m))
return BAD_VALUE;
const vec3_t up( getRotationMatrix() * Ba );
const vec3_t east( cross_product(m, up) );
vec3_t north( cross_product(up, east) );
const float l = 1 / length(north);
north *= l;
update(north, Bm, magSTDEV*l);
return NO_ERROR;
}
bool Fusion::checkState(const vec3_t& v) {
if (isnanf(length(v))) {
LOGW("9-axis fusion diverged. reseting state.");
P = 0;
x1 = 0;
mInitState = 0;
mCount[0] = 0;
mCount[1] = 0;
mCount[2] = 0;
mData = 0;
return false;
}
return true;
}
vec4_t Fusion::getAttitude() const {
return x0;
}
vec3_t Fusion::getBias() const {
return x1;
}
mat33_t Fusion::getRotationMatrix() const {
return quatToMatrix(x0);
}
mat34_t Fusion::getF(const vec4_t& q) {
mat34_t F;
F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y;
F[0].y = q.z; F[1].y = q.w; F[2].y =-q.x;
F[0].z =-q.y; F[1].z = q.x; F[2].z = q.w;
F[0].w =-q.x; F[1].w =-q.y; F[2].w =-q.z;
return F;
}
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);
// P(k+1) = F*P(k)*Ft + G*Q*Gt
// Phi = | Phi00 Phi10 |
// | 0 1 |
const mat33_t I33(1);
const mat33_t I33dT(dT);
const mat33_t wx(crossMatrix(we, 0));
const mat33_t wx2(wx*wx);
const float lwedT = length(we)*dT;
const float ilwe = 1/length(we);
const float k0 = (1-cosf(lwedT))*(ilwe*ilwe);
const float k1 = sinf(lwedT);
Phi[0][0] = I33 - wx*(k1*ilwe) + wx2*k0;
Phi[1][0] = wx*k0 - I33dT - wx2*(ilwe*ilwe*ilwe)*(lwedT-k1);
P = Phi*P*transpose(Phi) + GQGt;
}
void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) {
vec4_t q(x0);
// measured vector in body space: h(p) = A(p)*Bi
const mat33_t A(quatToMatrix(q));
const vec3_t Bb(A*Bi);
// Sensitivity matrix H = dh(p)/dp
// H = [ L 0 ]
const mat33_t L(crossMatrix(Bb, 0));
// gain...
// K = P*Ht / [H*P*Ht + R]
vec<mat33_t, 2> K;
const mat33_t R(sigma*sigma);
const mat33_t S(scaleCovariance(L, P[0][0]) + R);
const mat33_t Si(invert(S));
const mat33_t LtSi(transpose(L)*Si);
K[0] = P[0][0] * LtSi;
K[1] = transpose(P[1][0])*LtSi;
// update...
// P -= K*H*P;
const mat33_t K0L(K[0] * L);
const mat33_t K1L(K[1] * L);
P[0][0] -= K0L*P[0][0];
P[1][1] -= K1L*P[1][0];
P[1][0] -= K0L*P[1][0];
P[0][1] = transpose(P[1][0]);
const vec3_t e(z - Bb);
const vec3_t dq(K[0]*e);
const vec3_t db(K[1]*e);
q += getF(q)*(0.5f*dq);
x0 = normalize_quat(q);
x1 += db;
}
// -----------------------------------------------------------------------
}; // namespace android
Reference in New Issue View Git Blame Copy Permalink