This class is used to process and calculate attitude from imu sensors. More...

#include <ekf_imu13states.h>

Inheritance diagram for ekf_imu13states:

Collaboration diagram for ekf_imu13states:

Public Member Functions
	ekf_imu13states ()
virtual	~ekf_imu13states ()
virtual void	Init ()
virtual dspm::Mat	StateXdot (dspm::Mat &x, float *u)
virtual void	LinearizeFG (dspm::Mat &x, float *u)
void	Test ()
void	TestFull (bool enable_att)
void	UpdateRefMeasurement (float accel_data, float magn_data, float R[6])
void	UpdateRefMeasurementMagn (float accel_data, float magn_data, float R[6])
void	UpdateRefMeasurement (float accel_data, float magn_data, float *attitude, float R[10])
Public Member Functions inherited from ekf
	ekf (int x, int w)
virtual	~ekf ()
virtual void	Process (float *u, float dt)
void	RungeKutta (dspm::Mat &x, float *u, float dt)
virtual void	CovariancePrediction (float dt)
virtual void	Update (dspm::Mat &H, float measured, float expected, float *R)
virtual void	UpdateRef (dspm::Mat &H, float measured, float expected, float *R)

Data Fields
dspm::Mat	mag0
dspm::Mat	accel0
int	NUMU
Data Fields inherited from ekf
int	NUMX
int	NUMW
dspm::Mat &	X
dspm::Mat &	F
dspm::Mat &	G
dspm::Mat &	P
dspm::Mat &	Q
float *	HP
float *	Km

Additional Inherited Members
Static Public Member Functions inherited from ekf
static dspm::Mat	quat2rotm (float q[4])
static dspm::Mat	rotm2quat (dspm::Mat &R)
static dspm::Mat	quat2eul (const float q[4])
static dspm::Mat	eul2rotm (float xyz[3])
static dspm::Mat	rotm2eul (dspm::Mat &rotm)
static dspm::Mat	dFdq (dspm::Mat &vector, dspm::Mat &quat)
static dspm::Mat	dFdq_inv (dspm::Mat &vector, dspm::Mat &quat)
static dspm::Mat	SkewSym4x4 (float *w)
static dspm::Mat	qProduct (float *q)

Detailed Description

This class is used to process and calculate attitude from imu sensors.

The class use state vector with 13 follows values X[0..3] - attitude quaternion X[4..6] - gyroscope bias error, rad/sec X[7..9] - magnetometer vector value - magn_ampl X[10..12] - magnetometer offset value - magn_offset

where, reference magnetometer value = magn_ampl*rotation_matrix' + magn_offset

Definition at line 31 of file ekf_imu13states.h.

Constructor & Destructor Documentation

◆ ekf_imu13states()

ekf_imu13states::ekf_imu13states ( )

Definition at line 18 of file ekf_imu13states.cpp.

                                 : ekf(13, 18),
    mag0(3, 1),
    accel0(3, 1)
{
    this->NUMU = 3;
}

References accel0, ekf::ekf(), mag0, and NUMU.

Here is the call graph for this function:

◆ ~ekf_imu13states()

ekf_imu13states::~ekf_imu13states ( )

virtual

Definition at line 25 of file ekf_imu13states.cpp.

26{

27}

Member Function Documentation

◆ Init()

void ekf_imu13states::Init ( )

virtual

Initialization of EKF. The method should be called befare the first use of the filter.

Implements ekf.

Definition at line 29 of file ekf_imu13states.cpp.

{
    mag0.data[0] = 1;
    mag0.data[1] = 0;
    mag0.data[2] = 0;
 
    mag0 /= mag0.norm();
 
    accel0.data[0] = 0;
    accel0.data[1] = 0;
    accel0.data[2] = 1;
 
    accel0 /= accel0.norm();
 
    this->Q.Copy(0.1 * dspm::Mat::eye(3), 0, 0);
    this->Q.Copy(0.0001 * dspm::Mat::eye(3), 3, 3);
    this->Q.Copy(0.0001 * dspm::Mat::eye(3), 6, 6);
    this->Q.Copy(0.0001 * dspm::Mat::eye(3), 9, 9);
    this->Q.Copy(0.00001 * dspm::Mat::eye(3), 12, 12);
    this->Q.Copy(0.00001 * dspm::Mat::eye(3), 15, 15);
 
    this->X.data[0] = 1; // Init quaternion
    this->X.data[7] = 1; // Initial magnetometer vector
}

References accel0, dspm::Mat::eye(), mag0, ekf::Q, and ekf::X.

Here is the call graph for this function:

◆ LinearizeFG()

void ekf_imu13states::LinearizeFG	(	dspm::Mat &	x,
		float *	u )

virtual

Calculation of system state matrices F and G

Parameters

[in]	x	state vector
[in]	u	control measurement

Implements ekf.

Definition at line 75 of file ekf_imu13states.cpp.

{
    float w[3] = {(u[0] - x(4, 0)), (u[1] - x(5, 0)), (u[2] - x(6, 0))}; // subtract the biases on gyros
    // float w[3] = {u[0], u[1], u[2]}; // subtract the biases on gyros
 
    this->F *= 0; // Initialize F and G matrixes.
    this->G *= 0;
 
    // dqdot / dq - skey matrix
    F.Copy(0.5 * ekf::SkewSym4x4(w), 0, 0);
 
    // dqdot/dvector
    dspm::Mat dq = -0.5 * qProduct(x.data);
    dspm::Mat dq_q = dq.Get(0, 4, 1, 3);
 
    // dqdot / dnw
    G.Copy(dq_q, 0, 0);
    // dqdot / dwbias
    F.Copy(dq_q, 0, 4);
 
    dspm::Mat rotm = -1 * this->quat2rotm(x.data); // Convert quat to rotation matrix
 
    G.Copy(rotm, 7, 6);
    G.Copy(dspm::Mat::eye(3), 4, 3);   // random noise wbias
    G.Copy(dspm::Mat::eye(3), 7, 12);  // random noise magnetometer amplitude
    G.Copy(dspm::Mat::eye(3), 10, 9);  // magnetometer offset constant
    G.Copy(dspm::Mat::eye(3), 10, 15); // random noise offset constant
}

References dspm::Mat::data, dspm::Mat::eye(), ekf::F, ekf::G, dspm::Mat::Get(), ekf::qProduct(), ekf::quat2rotm(), ekf::SkewSym4x4(), and x.

Here is the call graph for this function:

◆ StateXdot()

dspm::Mat ekf_imu13states::StateXdot	(	dspm::Mat &	x,
		float *	u )

virtual

Derivative of state vector X The default calculation: xDot = F*x + G*u It's possible to implement optimized version

Parameters

[in]	x	state vector
[in]	u	control measurement

Returns: xDot - derivative of input vector x and u

Reimplemented from ekf.

Definition at line 54 of file ekf_imu13states.cpp.

{
    float wx = u[0] - x(4, 0); // subtract the biases on gyros
    float wy = u[1] - x(5, 0);
    float wz = u[2] - x(6, 0);
 
    float w[] = {wx, wy, wz};
    dspm::Mat q = dspm::Mat(x.data, 4, 1);
 
    // qdot = Q * w
    dspm::Mat Omega = 0.5 * SkewSym4x4(w);
    dspm::Mat qdot = Omega * q;
    dspm::Mat Xdot(this->NUMX, 1);
    Xdot *= 0;
    Xdot.Copy(qdot, 0, 0);
    // dwbias = 0
    // dMang_Ampl = 0
    // dMang_offset = 0
    return Xdot;
}

References dspm::Mat::Copy(), ekf::NUMX, ekf::SkewSym4x4(), and x.

Referenced by Test().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ Test()

void ekf_imu13states::Test ( )

Method for development and tests only.

Definition at line 104 of file ekf_imu13states.cpp.

{
    dspm::Mat test_x(7, 1);
    for (size_t i = 0; i < 7; i++) {
        test_x(i, 0) = i;
    }
    printf("Allocate data = %i\n", this->NUMU);
    float *test_u = new float[this->NUMU];
    for (size_t i = 0; i < this->NUMU; i++) {
        test_u[i] = i;
    }
    dspm::Mat result_StateXdot = StateXdot(test_x, test_u);
    delete[] test_u;
}

References NUMU, and StateXdot().

Here is the call graph for this function:

◆ TestFull()

void ekf_imu13states::TestFull ( bool enable_att )

Method for development and tests only.

Parameters

[in] enable_att - enable attitude as input reference value

Definition at line 119 of file ekf_imu13states.cpp.

{
    int total_N = 2048;
    float pi = std::atan(1) * 4;
    float gyro_err_data[] = {0.1, 0.2, 0.3}; // static constatnt error
    dspm::Mat gyro_err(gyro_err_data, 3, 1);
    float R[10];
    for (size_t i = 0; i < 10; i++) {
        R[i] = 0.01;
    }
 
    float accel0_data[] = {0, 0, 1};
    float magn0_data[] = {1, 0, 0};
 
    dspm::Mat accel0(accel0_data, 3, 1);
    dspm::Mat magn0(magn0_data, 3, 1);
 
    float dt = 0.01;
 
    dspm::Mat gyro_data(3, 1);
    int count = 0;
 
    // Initial rotation matrix
    dspm::Mat Rm = dspm::Mat::eye(3);
    dspm::Mat Re = dspm::Mat::eye(3);
 
    gyro_err *= 1;
 
    std::cout << "Gyro error: " << gyro_err.t() << std::endl;
    for (size_t n = 1; n < total_N * 3; n++) {
        if ((n % 1000) == 0) {
            std::cout << "Loop " << n << " from " << total_N * 16;
            std::cout << ", State data : " << this->X.t();
        }
        gyro_data *= 0; // reset gyro value
        if ((n >= (total_N / 2)) && (n < total_N * 12)) {
            gyro_data(0, 0) = 1 / pi * std::cos(-pi / 2 + pi / 2 * count * 2 / (total_N / 10));
            gyro_data(1, 0) = 2 / pi * std::cos(-pi / 2 + pi / 2 * count * 2 / (total_N / 10));
            gyro_data(2, 0) = 3 / pi * std::cos(-pi / 2 + pi / 2 * count * 2 / (total_N / 10));
            count++;
        }
        dspm::Mat gyro_sample = gyro_data + gyro_err;
 
        gyro_data *= dt;
        Re = this->eul2rotm(gyro_data.data); // Calculate rotation to gyro angel
        Rm = Rm * Re;                        // Rotate original matrix
        dspm::Mat attitude = ekf::rotm2quat(Rm);
        // We have to rotate accel and magn to the opposite direction
        dspm::Mat accel_data = Rm.t() * accel0;
        dspm::Mat magn_data = Rm.t() * magn0;
 
        dspm::Mat accel_norm = accel_data / accel_data.norm();
        dspm::Mat magn_norm = magn_data / magn_data.norm();
 
        float input_u[] = {gyro_sample(0, 0), gyro_sample(1, 0), gyro_sample(2, 0)};
        // Process input values to new state
        this->Process(input_u, dt);
        dspm::Mat q_norm(this->X.data, 4, 1);
        q_norm /= q_norm.norm();
 
        if (true == enable_att) {
            this->UpdateRefMeasurement(accel_norm.data, magn_norm.data, attitude.data, R);
        } else {
            this->UpdateRefMeasurement(accel_norm.data, magn_norm.data, R);
        }
    }
    std::cout << "Final State data : " << this->X.t() << std::endl;
}

References accel0, dspm::Mat::data, ekf::eul2rotm(), dspm::Mat::eye(), n, dspm::Mat::norm(), ekf::Process(), ekf::rotm2quat(), dspm::Mat::t(), UpdateRefMeasurement(), and ekf::X.

Here is the call graph for this function:

◆ UpdateRefMeasurement() [1/2]

void ekf_imu13states::UpdateRefMeasurement	(	float *	accel_data,
		float *	magn_data,
		float *	attitude,
		float	R[10] )

Update system state by reference measurements accelerometer, magnetometer and attitude quaternion. This method could be used when system on constant state or in initialization phase.

Parameters

[in]	accel_data	accelerometer measurement vector XYZ in g, where 1 g ~ 9.81 m/s^2
[in]	magn_data	magnetometer measurement vector XYZ
[in]	attitude	attitude quaternion
[in]	R	measurement noise covariance values for diagonal covariance matrix. Then smaller value, then more you trust them.

Definition at line 256 of file ekf_imu13states.cpp.

{
    dspm::Mat quat(this->X.data, 4, 1);
    dspm::Mat H = 0 * dspm::Mat(10, this->NUMX);
    dspm::Mat Re = this->quat2rotm(quat.data).t();
 
    H.Copy(Re, 0, 7);
    H.Copy(dspm::Mat::eye(3), 0, 10);
    // dAccel/dq
    dspm::Mat dAccel_dq = ekf::dFdq_inv(this->accel0, quat);
    H.Copy(dAccel_dq, 3, 0);
    // dMagn/dq
    dspm::Mat magn(&this->X.data[7], 3, 1);
    dspm::Mat magn_offset(&this->X.data[10], 3, 1);
    dspm::Mat dMagn_dq = ekf::dFdq_inv(magn, quat);
    H.Copy(dMagn_dq, 0, 0);
 
    // dq/dq
    H.Copy(dspm::Mat::eye(4), 6, 1);
 
    dspm::Mat expected_magn = Re * magn + magn_offset;
    dspm::Mat expected_accel = Re * this->accel0;
 
    float measured_data[10];
    float expected_data[10];
    for (size_t i = 0; i < 3; i++) {
        measured_data[i] = magn_data[i];
        expected_data[i] = expected_magn.data[i];
        measured_data[i + 3] = accel_data[i];
        expected_data[i + 3] = expected_accel.data[i];
    }
    for (size_t i = 0; i < 4; i++) {
        measured_data[i + 6] = attitude[i];
        expected_data[i + 6] = this->X.data[i];
    }
 
    this->Update(H, measured_data, expected_data, R);
    quat /= quat.norm();
}

References accel0, dspm::Mat::Copy(), dspm::Mat::data, ekf::dFdq_inv(), dspm::Mat::eye(), dspm::Mat::norm(), ekf::NUMX, ekf::quat2rotm(), dspm::Mat::t(), ekf::Update(), and ekf::X.

Here is the call graph for this function:

◆ UpdateRefMeasurement() [2/2]

void ekf_imu13states::UpdateRefMeasurement	(	float *	accel_data,
		float *	magn_data,
		float	R[6] )

Update part of system state by reference measurements accelerometer and magnetometer. Only attitude and gyro bias will be updated. This method should be used as main method after calibration.

Parameters

[in]	accel_data	accelerometer measurement vector XYZ in g, where 1 g ~ 9.81 m/s^2
[in]	magn_data	magnetometer measurement vector XYZ
[in]	R	measurement noise covariance values for diagonal covariance matrix. Then smaller value, then more you trust them.

Definition at line 188 of file ekf_imu13states.cpp.

{
    dspm::Mat quat(this->X.data, 4, 1);
    dspm::Mat H = 0 * dspm::Mat(6, this->NUMX);
    dspm::Mat Re = this->quat2rotm(quat.data).t();
 
    // dAccel/dq
    dspm::Mat dAccel_dq = ekf::dFdq_inv(this->accel0, quat);
    H.Copy(dAccel_dq, 3, 0);
 
    // dMagn/dq
    dspm::Mat magn(&this->X.data[7], 3, 1);
    dspm::Mat magn_offset(&this->X.data[10], 3, 1);
    dspm::Mat dMagn_dq = ekf::dFdq_inv(magn, quat);
    H.Copy(dMagn_dq, 0, 0);
 
    dspm::Mat expected_magn = Re * magn + magn_offset;
    dspm::Mat expected_accel = Re * this->accel0;
 
    float measured_data[6];
    float expected_data[6];
    for (size_t i = 0; i < 3; i++) {
        measured_data[i] = magn_data[i];
        expected_data[i] = expected_magn.data[i];
        measured_data[i + 3] = accel_data[i];
        expected_data[i + 3] = expected_accel.data[i];
    }
 
    this->Update(H, measured_data, expected_data, R);
    quat /= quat.norm();
}

References accel0, dspm::Mat::Copy(), dspm::Mat::data, ekf::dFdq_inv(), dspm::Mat::norm(), ekf::NUMX, ekf::quat2rotm(), dspm::Mat::t(), ekf::Update(), and ekf::X.

Referenced by TestFull().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ UpdateRefMeasurementMagn()

void ekf_imu13states::UpdateRefMeasurementMagn	(	float *	accel_data,
		float *	magn_data,
		float	R[6] )

Update full system state by reference measurements accelerometer and magnetometer. This method should be used at calibration phase.

Parameters

[in]	accel_data	accelerometer measurement vector XYZ in g, where 1 g ~ 9.81 m/s^2
[in]	magn_data	magnetometer measurement vector XYZ
[in]	R	measurement noise covariance values for diagonal covariance matrix. Then smaller value, then more you trust them.

Definition at line 220 of file ekf_imu13states.cpp.

{
    dspm::Mat quat(this->X.data, 4, 1);
    dspm::Mat H = 0 * dspm::Mat(6, this->NUMX);
    dspm::Mat Re = this->quat2rotm(quat.data).t();
 
    // We include these two line to update magnetometer initial state
    H.Copy(Re, 0, 7);
    H.Copy(dspm::Mat::eye(3), 0, 10);
 
    // dAccel/dq
    dspm::Mat dAccel_dq = ekf::dFdq_inv(this->accel0, quat);
    H.Copy(dAccel_dq, 3, 0);
 
    // dMagn/dq
    dspm::Mat magn(&this->X.data[7], 3, 1);
    dspm::Mat magn_offset(&this->X.data[10], 3, 1);
    dspm::Mat dMagn_dq = ekf::dFdq_inv(magn, quat);
    H.Copy(dMagn_dq, 0, 0);
 
    dspm::Mat expected_magn = Re * magn + magn_offset;
    dspm::Mat expected_accel = Re * this->accel0;
 
    float measured_data[6];
    float expected_data[6];
    for (size_t i = 0; i < 3; i++) {
        measured_data[i] = magn_data[i];
        expected_data[i] = expected_magn.data[i];
        measured_data[i + 3] = accel_data[i];
        expected_data[i + 3] = expected_accel.data[i];
    }
 
    this->Update(H, measured_data, expected_data, R);
    quat /= quat.norm();
}

References accel0, dspm::Mat::Copy(), dspm::Mat::data, ekf::dFdq_inv(), dspm::Mat::eye(), dspm::Mat::norm(), ekf::NUMX, ekf::quat2rotm(), dspm::Mat::t(), ekf::Update(), and ekf::X.

Here is the call graph for this function:

Field Documentation

◆ accel0

dspm::Mat ekf_imu13states::accel0

Initial reference valie for accelerometer.

Definition at line 60 of file ekf_imu13states.h.

Referenced by ekf_imu13states(), Init(), TestFull(), UpdateRefMeasurement(), UpdateRefMeasurement(), and UpdateRefMeasurementMagn().

◆ mag0

dspm::Mat ekf_imu13states::mag0

Initial reference valie for magnetometer.

Definition at line 56 of file ekf_imu13states.h.

Referenced by ekf_imu13states(), and Init().

◆ NUMU

int ekf_imu13states::NUMU

number of control measurements

Definition at line 65 of file ekf_imu13states.h.

Referenced by ekf_imu13states(), and Test().

The documentation for this class was generated from the following files:

Public Member Functions

Data Fields

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ ekf_imu13states()

◆ ~ekf_imu13states()

Member Function Documentation

◆ Init()

◆ LinearizeFG()

◆ StateXdot()

◆ Test()

◆ TestFull()

◆ UpdateRefMeasurement() [1/2]

◆ UpdateRefMeasurement() [2/2]

◆ UpdateRefMeasurementMagn()

Field Documentation

◆ accel0

◆ mag0

◆ NUMU