# How do I 'fuse' together two different kinds of data to get a final result?

I am building a robot (2 powered wheels and one ball bearing). The problem is that I can't seem to make it drive straight. I literally find it impossible, I have been trying for weeks.

Currently I am able to rely on rotations (of both motors) or the gyro readings(I also have two gyros, each near the two tyres)

Is there a way I can fuse those together, giving me a more accurate way to determine which motor I need to speed up?

My motors accept a value from 0-900 (although the speed should be determined by me and not fixed). Also if an algorithm exists, I'd like some directions of what I'd need to swap if I make the motors go backwards.

• What type of a gyro do you have, or more specifically what parameters are available? Driving on a straight line was discussed on Robotics SE, it could help a lot. May 20, 2018 at 15:31
• I have angle and rotation speed. The gyro is not super precise on it's own. May 20, 2018 at 15:34
• What type of motors and do they have encoders? May 20, 2018 at 16:23
• Can all motors be configured or set differently to each other? May 24, 2018 at 20:51

The robot is unable to drive straight because no two motors are exact. Given the same voltage and target speed, both motors will have slight difference from one another which will accumulate over time to make the robot move in a non-straight direction.

What kind of feedback control are you implementing? A simple PID control should work fine for adjusting the speed since you have rotation and speed feedback from both the motors. A very good explanation and example is given at this website. While it is for lego, but the concept is the same for the PID control. You can also see the code implementation from my code at GitHub (se function calculateTurnDirDistandSpeed).

I'm assuming you know what to do with the "single input" once you have it, so I'm not going to talk about control algorithms - just how to aggregate the inputs.

Your input is two gyros, providing in perfect conditions the same exact value; the values will differ though due to various 'inadequacies' of the world.

One simple method is averaging their data. Sum, divide by two.

If your input data is noisy, use a smoothing algorithm (weighted moving average, WMA). I like the ARX model which is computationally very lightweight and provides nice, well-tunable results: $\text{WMA}_n = w \ p_n + (1-w) \ \text{WMA}_{n-1}$ where $p_n$ is the current sample, $\text{WMA}_n$ is the result ready for further use, $W_{n-1}$ is the previous result (obtained from previous sample), and $w$ is a smoothing factor, $0 < w ≪ 1$, higher values like 0.1-0.2 provide faster reaction, lower values like 0.001 provide good smoothing. Just mix all these samples taking them from both inputs, never caring whether current p_n comes from one gyro or the other, and it will work.

Or you can take a more advanced approach, e.g. evaluate the reliability of inputs. For that you need at least three values - but we can manufacture the third one, using e.g. the same ARX model as above. Keep calculating the moving average as above, but don't use it for control. Instead, compare the two inputs to it, and reject the farther one. This helps a lot with signal that has short high spikes, e.g. from EM pulse-type noise, static discharges - or terrain bumps.

Also, independently, you can include fault detection and redundancy. Keep track of how much the two inputs differ from each other and if they do differ by much for a long time (again, WMA of the difference, $|p^A_i - p^B_i|$, with a long time constant), you have a condition of a sensor fault. You can estimate which sensor by keeping two WMAs, each separate for one sensor, summing the differences between readouts and average over a "typical" time and comparing to typical values (obtained experimentally). $F = { 1 \over n-k} \sum_{i=k}^{n} |\text{WMA}_i - p_i|$. Very low $F$ means the sensor is stuck and producing a constant; very high $F$ means the sensor broke and produces white noise. Generally, the $F$ further from typical will indicate the faulty sensor. Report the fault and exclude the faulty sensor from further use until the error condition is gone.