# Algorithm problem **URL:** \ **Category:** Sensors\ **Created:** [November 15, 2023, 4:58pm UTC](https://forum.arduino.cc/t/algorithm-problem/1189504 "2023-11-15T16:58:11Z")\ **Posts on this page:** 6\ **Page:** 1
**Author:** ![masteriphone](https://dub1.discourse-cdn.com/arduino/user_avatar/forum.arduino.cc/masteriphone/32/1003454_2.png) [@masteriphone](https://forum.arduino.cc/u/masteriphone)\ **Post date:** [November 15, 2023, 4:58pm UTC](https://forum.arduino.cc/t/algorithm-problem/1189504/1 "2023-11-15T16:58:11Z")
I am searching for the optimal orientation algorithm for the device. I found a code on Github that was written for C and the author of the code does not support his brainchild. I managed to rewrite the code for the Arduino platform and add my lsm303dlh and l3gd20 sensors. In general, I wrote about the problem in [issue](https://github.com/rbv188/IMU-algorithm/issues/3) and have attached an [archive](https://github.com/rbv188/IMU-algorithm/files/13366976/IMU-algorithm.zip) with the source code to Github. I would like help with how to change the axes in the code, and I also couldn’t find how to change the influence of the gyroscope and accelerometer on orientation angles. In theory, the code based on the DCM should somehow establish the proportions between the gyroscope and the accelerometer so that for short periods of time during movement, the orientation angles do not react to sudden movements and centripetal accelerations. Now the code works terribly; with any sudden movement, the roll and pitch angles also jerk. Such an IMU filter is useless. ---
**Author:** ![anon94801460](https://avatars.discourse-cdn.com/v4/letter/a/278dde/32.png) [@anon94801460](https://forum.arduino.cc/u/anon94801460)\ **Post date:** [November 15, 2023, 5:11pm UTC](https://forum.arduino.cc/t/algorithm-problem/1189504/2 "2023-11-15T17:11:27Z")
The DCM approach has mostly been abandoned in favor of the Mahony and Madgwick filters, which most people use on small MCUs. If you are careful about mapping the sensor coordinate axes, ensuring a right handed, consistent coordinate system, it is no problem to implement either with the sensors you have. ---
**Author:** ![masteriphone](https://dub1.discourse-cdn.com/arduino/user_avatar/forum.arduino.cc/masteriphone/32/1003454_2.png) [@masteriphone](https://forum.arduino.cc/u/masteriphone)\ **Post date:** [November 15, 2023, 5:24pm UTC](https://forum.arduino.cc/t/algorithm-problem/1189504/3 "2023-11-15T17:24:39Z")
Dear Jremington. Regarding the DCM, I wrote a little off topic, because ... in the above code, it’s the quaternions that do all the work. But in this implementation of the code, I cannot understand how the directions of the accelerometer axes and the direction of rotation of the gyroscope are set. This is actually what prompted the need to write to the forum. The only thing that even somehow hints at the initial parameters are these [two lines](https://github.com/rbv188/IMU-algorithm/blob/bd77afdb7018a46998ae74e12bcd763c5b6711b2/IMU-algorithm-master.ino#L30). ```arduino fused_vector = vector_3d_initialize(0.0,0.0,-1.0); q_acc = quaternion_initialize(1.0,0.0,0.0,0.0); ``` But changing their parameters does not lead to anything. Not long ago I took your code > [@Controlling Servo with MPU6050](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/34): > > For full 3D orientation, most people use a 9DOF sensor, because the MPU-6050 can't define yaw (angle from North). However, for pitch and roll stabilization with your MPU-6050. you can start with this code, which uses the Mahony filter to determine the 3D orientation. Yaw will drift. Be sure to calibrate the rate gyro as every one is different. It is not so important to calibrate the accelerometer, but it helps. This code just prints out the orientation angles. // // MPU-6050 Mahony IMU  (yaw… And even though it is small, it works perfectly and with correctly selected coefficients Kp and Ki, it completely compensates for sudden movements to the right and left and back and forth. I’m just interested in mastering the topic of DCM and Kalman filtering for personal development, because more serious IMUs with good microcontrollers still use Kalman filters and matrices with vectors. Knowledge of quaternions alone is no longer enough 😀 ---
**Author:** ![anon94801460](https://avatars.discourse-cdn.com/v4/letter/a/278dde/32.png) [@anon94801460](https://forum.arduino.cc/u/anon94801460)\ **Post date:** [November 15, 2023, 5:26pm UTC](https://forum.arduino.cc/t/algorithm-problem/1189504/4 "2023-11-15T17:26:31Z")
> [@masteriphone](#): > > the directions of the accelerometer axes and the direction of rotation of the gyroscope are set. Those directions are fixed, as clearly explained using diagrams, in the sensor data sheets. However, it is straightforward to remap the manufacturer-assigned axes. > it’s the quaternions that do all the work Well, quaternion math, which is different only in notation and convention, from any other matrix math formulation. All quaternion operations can be performed with conventional matrices and vectors. ---
**Author:** ![masteriphone](https://dub1.discourse-cdn.com/arduino/user_avatar/forum.arduino.cc/masteriphone/32/1003454_2.png) [@masteriphone](https://forum.arduino.cc/u/masteriphone)\ **Post date:** [November 15, 2023, 5:45pm UTC](https://forum.arduino.cc/t/algorithm-problem/1189504/5 "2023-11-15T17:45:17Z")
I don’t know why it wasn’t possible to redefine the direction of the accelerometer axis before, but now everything works. As usual, if the acceleration axis is facing UP, then the force of gravity is directed DOWN and you just need to put a “-” in front of the acceleration value in the code: > I also found two options for calculating the roll angle from quaternion, in the source code there was the first option ```arduino result.roll = atan2(2*(q0*q1 + q2*q3),1 - 2*(q1*q1 + q2*q2))*180/3.14; // result.roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)*180/3.14; ``` and in the code you specified, there was a slightly different one. > [@Controlling Servo with MPU6050](https://forum.arduino.cc/t/controlling-servo-with-mpu6050/685306/34): > > For full 3D orientation, most people use a 9DOF sensor, because the MPU-6050 can't define yaw (angle from North). However, for pitch and roll stabilization with your MPU-6050. you can start with this code, which uses the Mahony filter to determine the 3D orientation. Yaw will drift. Be sure to calibrate the rate gyro as every one is different. It is not so important to calibrate the accelerometer, but it helps. This code just prints out the orientation angles. // // MPU-6050 Mahony IMU  (yaw… But the result is not much different. I'm looking for algorithms that will deal with changes in roll or pitch angles during sudden movements of the device. [Video1](https://disk.yandex.com/i/sg-nLmTnDR3kbQ) and [video2](https://disk.yandex.com/i/WU6OBnVsJ4jPeg) And this [video3](https://disk.yandex.com/i/94_koT7Prvw9jA) shows the operation of DCM from open source > The video shows that the algorithm successfully compensates for sideways jerks of the sensor, while the orientation angles remain stable. ---
**Author:** ![system](https://dub1.discourse-cdn.com/arduino/user_avatar/forum.arduino.cc/system/32/1140315_2.png) [@system](https://forum.arduino.cc/u/system)\ **Post date:** [May 13, 2024, 5:45pm UTC](https://forum.arduino.cc/t/algorithm-problem/1189504/6 "2024-05-13T17:45:55Z")
This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.