姿态融合的一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序

文档内容节选

一阶互补 atau tau loop time newAngle angle measured with atan2 using the accelerometer加速度传感器输出值 newRate angle measured using the gyro looptime loop time in millisfloat tau 0075float a 00float Complementaryfloat newAngle float newRate int looptime float dtC floatlooptime 10000 a tau tau dtC xangleC a xangleC newRate dtC 1 a newAngle return xangleC二阶互补 newAngle angle measured with atan2 using the accelerometer newRate angle measured using the gyro looptime loop time in millis......