9軸センサで姿勢推定したい

Navio2の3軸ジャイロセンサと3軸加速度センサ用いて誤差を補正しながら、姿勢推定を行いたいです。またここに3軸地磁気センサを加えることで、ヨー方向の誤差も補正したいです。計算の手順はわかったのですが、実際に参考にできるプログラムが見つからなくて困っています。言語はなんでもいいので実際に9軸センサで姿勢推定をしているプログラムかサイトを教えてほしいです。

サイトはここを参考にしました

https://qiita.com/Tanba28/items/5092c3e5e2c631b804f3

9軸のセンサ情報は以下のプログラムで出力しています
加速度：ax,ay,az
角速度：gx,gy,gz
磁気：mx,my,mz

C++
1/*
2Provided to you by Emlid Ltd (c) 2015.
3twitter.com/emlidtech || www.emlid.com || info@emlid.com
4Example: Read accelerometer, gyroscope and magnetometer values from
5inertial measurement unit: MPU9250 or LSM9DS1 over SPI on Raspberry Pi + Navio.
6Navio's onboard sensors are connected to the SPI bus on Raspberry Pi
7and can be read through /dev/spidev0.1 (MPU9250), /dev/spidev0.3 (acc/gyro LSM9DS1)
8and /dev/spidev0.2 (mag LSM9DS1).
9To run this example navigate to the directory containing it and run following commands:
10make
11./AccelGyroMag -i [sensor name]
12Sensors names: mpu is MPU9250, lsm is LSM9DS1.
13For print help:
14./AccelGyroMag -h
15*/
16
17#include "Common/MPU9250.h"
18#include "Navio2/LSM9DS1.h"
19#include "Common/Util.h"
20#include <unistd.h>
21#include <string>
22#include <memory>
23
24std::unique_ptr <InertialSensor> get_inertial_sensor( std::string sensor_name)
25{
26    if (sensor_name == "mpu") {
27        printf("Selected: MPU9250\n");
28        auto ptr = std::unique_ptr <InertialSensor>{ new MPU9250() };
29        return ptr;
30    }
31    else if (sensor_name == "lsm") {
32        printf("Selected: LSM9DS1\n");
33        auto ptr = std::unique_ptr <InertialSensor>{ new LSM9DS1() };
34        return ptr;
35    }
36    else {
37        return NULL;
38    }
39}
40
41void print_help()
42{
43    printf("Possible parameters:\nSensor selection: -i [sensor name]\n");
44    printf("Sensors names: mpu is MPU9250, lsm is LSM9DS1\nFor help: -h\n");
45}
46
47std::string get_sensor_name(int argc, char *argv[])
48{
49    if (get_navio_version() == NAVIO2) {
50
51        if (argc < 2) {
52            printf("Enter parameter\n");
53            print_help();
54            return std::string();
55        }
56
57        // prevent the error message
58        opterr = 0;
59        int parameter;
60
61        while ((parameter = getopt(argc, argv, "i:h")) != -1) {
62            switch (parameter) {
63            case 'i': return optarg;
64            case 'h': print_help(); return "-1";
65            case '?': printf("Wrong parameter.\n");
66                      print_help();
67                      return std::string();
68            }
69        }
70
71    } else { //sensor on NAVIO+
72
73        return "mpu";
74    }
75
76    return std::string();
77}
78//=============================================================================
79int main(int argc, char *argv[])
80{
81
82    if (check_apm()) {
83        return 1;
84    }
85
86    auto sensor_name = get_sensor_name(argc, argv);
87    if (sensor_name.empty())
88        return EXIT_FAILURE;
89
90    auto sensor = get_inertial_sensor(sensor_name);
91
92    if (!sensor) {
93        printf("Wrong sensor name. Select: mpu or lsm\n");
94        return EXIT_FAILURE;
95    }
96
97    if (!sensor->probe()) {
98        printf("Sensor not enabled\n");
99        return EXIT_FAILURE;
100    }
101    sensor->initialize();
102
103    float ax, ay, az;
104    float gx, gy, gz;
105    float mx, my, mz;
106//-------------------------------------------------------------------------
107
108    while(1) {
109        sensor->update();
110        sensor->read_accelerometer(&ax, &ay, &az);
111        sensor->read_gyroscope(&gx, &gy, &gz);
112        sensor->read_magnetometer(&mx, &my, &mz);
113        printf("Acc: %+7.3f %+7.3f %+7.3f  ", ax, ay, az);
114        printf("Gyr: %+8.3f %+8.3f %+8.3f  ", gx, gy, gz);
115        printf("Mag: %+7.3f %+7.3f %+7.3f\n", mx, my, mz);
116
117       usleep(500000);
118    }
119}