如何计算倾斜补偿偏航？

Question

我已经挣扎了一段时间了，关于如何正确地计算IMU的偏航角度，但就是不能让它工作。我正在使用LSM9DS1，如果这很重要的话。

我已经有了适当的滚动值和俯仰值。在我开始倾斜设备之前，偏航的值也或多或少是正确的。因此，我必须实现某种倾斜补偿。

我按如下方式计算欧拉角：

    double weight = 0.05f;

    private void calculateEulerAngles() {
        // Measured angle by the accelerometer
        double rollXLMeasured = Math.atan2(this.accelerometer.getX(), this.accelerometer.getZ()) / 2 / Math.PI * 360;
        double pitchXLMeasured = Math.atan2(this.accelerometer.getY() / 9.81f, this.accelerometer.getZ() / 9.81f) / 2 / Math.PI * 360;

        // Adding a low pass filter
        double rollXLFiltered = (1 - this.weight) * rollXLFilteredOld + this.weight * rollXLMeasured;
        double pitchXLFiltered = (1 - this.weight) * pitchXLFilteredOld + this.weight * pitchXLMeasured;
        this.rollXLFilteredOld = rollXLFiltered;
        this.pitchXLFilteredOld = pitchXLFiltered;

        // Calculating deltaTime
        long time = System.nanoTime();
        int difference = (int) ((time - this.deltaTime) / 1000000000);
        this.deltaTime = time;

        // Adding a complementary filter
        this.roll = ((1 - this.weight) * (this.roll + this.gyroscope.getY() * difference)) + (this.weight * rollXLMeasured);
        this.pitch = ((1 - this.weight) * (this.pitch - this.gyroscope.getX() * difference)) + (this.weight * pitchXLMeasured);

        // Calculating yaw using the magnetometer and applying a low pass filter
        double rollRadians = this.roll / 360 * (2 * Math.PI);
        double pitchRadians = this.pitch / 360 * (2 * Math.PI);
        double magnetometerXCompensated = (-this.magnetometer.getX() * Math.cos(rollRadians)) - (this.magnetometer.getY() * Math.sin(pitchRadians) *
                Math.sin(rollRadians)) + (this.magnetometer.getZ() * Math.cos(pitchRadians) * Math.sin(rollRadians));
        double magnetometerYCompensated = (this.magnetometer.getY() * Math.cos(pitchRadians)) + (this.magnetometer.getZ() * Math.sin(pitchRadians));
        double yawMeasured = Math.atan2(magnetometerYCompensated, magnetometerXCompensated) / (2 * Math.PI) * 360;
        double yawFiltered = (1 - this.weight) * yawFilteredOld + this.weight * yawMeasured;
        this.yawFilteredOld = yawFiltered;
        this.yaw = yawFiltered;

        // Print roll, pitch and yaw for debug purposes
        System.out.println(this.roll + ", " + this.pitch + ", " + this.yaw);
    }

我没有包括我使用的全部代码，因为我认为上面的代码做什么很清楚，我想这是问题的关键部分。

再一次，我得到了正确的值，只是当我倾斜设备时不是偏航。所以一定有一个关于数学的错误。

我是否必须使用原始值进行计算，比如IMU中的数据，或者使用已经处理过的数据？例如，this.accelerometer.getX()实际上返回x_raw * 0.061f / 1000 * 9.81f，其中x_raw是存储在IMU中的值，0.061f是一些系数。我基本上复制了Adafruit库中的计算，我不是100%确定为什么你必须乘/除这些值。

此外，您可能已经注意到，当我计算magnetometerXCompensated值时，我反转了x轴。我这样做，是因为磁力计的轴没有与加速计/陀螺仪轴对齐，所以为了对齐它们，我必须反转x轴。

有没有人有办法解决这个问题？我真的厌倦了它不能正常工作，因为我现在尝试了很长一段时间来解决它，但我没有得到我想要的结果。

Answer 1

你可以从下一张图中得到所有的方程式。互补/卡尔曼滤波器在那里是为了得到更少的噪声。

​