Getting Started

The aim of the this section is to provide runnable minimum working examples that show how the orientation estimation filter can be used and that can serve as a starting point for real use cases.

To create a simple test case that does not require access to real IMU measurements, simple dummy data is used that consists of a constant gyroscope measurement of 1 °/s in all axes and a constant accelerometer measurement with norm 9.81 m/s² in direction [1 1 1]. To keep it as simple as possible, magnetometers are not used. This data is fed to the algorithm—once to the full version and once to the basic version—and the resulting orientation quaternion is plotted.

It is worth noticing that this example already nicely demonstates the effect of gyroscope bias estimation. After a short time, the full version estimates and removes the gyroscope bias which leads to a constant quaternion. In contrast, the quaternion estimated by the basic version always changes (mostly due to rotation around the vertical axis because magnetometer measurements are not used).

Python

import numpy as np
import matplotlib.pyplot as plt
from vqf import VQF, BasicVQF, PyVQF

# generate simple dummy data
gyr = np.deg2rad(1)*np.ones((6000, 3))  # in rad/s
acc = 9.81/np.sqrt(3)*np.ones((6000, 3))  # in m/s²
Ts = 0.01  # sampling time (100 Hz)

# run orientation estimation
vqf = VQF(Ts)
# alternative: vqf = PyVQF(Ts)
out = vqf.updateBatch(gyr, acc)

# plot the quaternion
plt.figure()
plt.subplot(211)
plt.plot(out['quat6D'])
plt.title('quaternion (full version)')
plt.grid()

# run the basic version with the same data
params = dict(
    motionBiasEstEnabled=False,
    restBiasEstEnabled=False,
    magDistRejectionEnabled=False,
)
vqf2 = VQF(Ts, **params)
# alternative: vqf2 = BasicVQF(Ts)
# alternative: vqf2 = PyVQF(Ts, **params)
out2 = vqf2.updateBatch(gyr, acc)

# plot quaternion (notice the difference due to disabled bias estimation)
plt.subplot(212)
plt.plot(out2['quat6D'])
plt.grid()
plt.title('quaternion (basic version)')
plt.tight_layout()
plt.show()

(Source code, png, hires.png, pdf)

C++

The following small C++ program calculates the same values as the Python example and outputs a csv file:

#include "vqf.hpp"

#include <iostream>

void run(bool basic) {
    vqf_real_t gyr[3] = {0.01745329, 0.01745329, 0.01745329}; // 1 °/s in rad/s
    vqf_real_t acc[3] = {5.663806, 5.663806, 5.663806}; // 9.81/sqrt(3) m/s²
    vqf_real_t quat[4] = {0, 0, 0, 0}; // output array for quaternion

    VQFParams params;

    if (basic) { // alternative: use the BasicVQF class
        params.restBiasEstEnabled = false;
        params.motionBiasEstEnabled = false;
        params.magDistRejectionEnabled = false;
    }

    VQF vqf(params, 0.01);
    for (size_t i=0; i < 6000; i++) {
        vqf.update(gyr, acc);
        vqf.getQuat6D(quat);
        std::cout << int(basic) << ", " << i << ", "<< quat[0] << ", " << quat[1]
                  << ", "<< quat[2] << ", " << quat[3] << std::endl;
    }
}

int main()
{
    run(false);
    run(true);
    return 0;
}

Matlab

The following small Matlab script calculates the same values as the Python example and shows an equivalent plot:

%% generate simple dummy data
gyr = deg2rad(1)*ones(5000, 3);  % in rad/s
acc = 9.81/sqrt(3)*ones(5000, 3); % in m/s²
Ts = 0.01; % sampling time (100 Hz)

%% run orientation estimation
vqf = VQF(Ts);
out = vqf.updateBatch(gyr, acc);

%% plot the quaternion
figure();
subplot(211);
plot(out.quat6D);
title('quaternion (full version)');
grid();

%% run the basic version with the same data
params = struct();
params.motionBiasEstEnabled = false;
params.restBiasEstEnabled = false;
params.magDistRejectionEnabled = false;
vqf2 = VQF(Ts, params);
out2 = vqf2.updateBatch(gyr, acc);

%% plot quaternion (notice the difference due to disabled bias estimation)
subplot(212);
plot(out2.quat6D);
grid();
title('quaternion (basic version)');