# Filtering encoder values in moteus

TLDR: moteus can now filter the encoder, resulting in less audible noise. Use firmware version 2021-04-20 and ‘pip3 install moteus’ version 0.3.19, then re-calibrate to get the benefits.

## Background

The moteus controller uses an absolute magnetic encoder to measure the position of the rotor. It uses this knowledge to accurately control the current through the three phases of a brushless motor so that the desired torque is produced, i.e. “field oriented control”. This works well, but has some downsides. One, is that magnetic encoders work by sensing the magnetic field produced by a “sensing magnet” that is somehow affixed to the rotor. This sensing process always introduces some noise, so that the sensed rotor position is never perfect.

Because of this, even if the rotor is perfectly stationary, moteus will constantly be tweaking the phase of the motor current to track the noise. This results in slightly increased power consumption, and more important to most, audible noise as the slight variations in control current can induce vibrations in the phase windings of the motor.

Fortunately, in most applications, the rotor is not actually capable of accelerating with the full bandwidth that the magnetic sensor is capable of sensing. Thus, we can filter it to remove high frequency noise without any loss of performance.

## Approach

The filter method that moteus uses is an “all-digital phase locked loop“. Formulated in the traditional PLL terminology, this consists of three pieces: a numerically controlled oscillator, a phase detector, and a PI controller.

The “numerically controlled oscillator” is just a counter that tries to “guess” what the encoder is doing by propagating a value forward at an estimated velocity. For an ADPLL in this setup (where our encoder gives us an absolute phase measurement), the “phase detector” just subtracts the measured phase from the estimated phase. The PI controller determines how the estimated oscillator tracks disturbances in the actual encoder frequency.

In my post on automatic torque bandwidth selection for moteus, I derived the necessary equations for determining the torque bandwidth. Here, I’m just going to reference the excellent article by Neil Robertson, “Digital PLL’s” (part 1, and part 2), over at dsprelated.com. There, he derived the necessary proportional and derivative gains for a given bandwidth and damping ratio:

$K_p = 2 \zeta \omega_{3db}$

$K_i = {\omega_{3db}}^ 2$

Where $\zeta$ is the “damping ratio“. For our purposes, we’ll use 1.0, which is known as “critically damped” and is often a good balance between stability and response time.

## Experiments

To see this in action, I ran some experiments on a moteus devkit. I added back in a conditional ability to emit a few bytes of debugging information at the full control rate to the firmware, and used that to emit the encoder value as used by FOC at each control cycle. I made up a script, that can plot the power spectral density in the encoder measurements. This is basically the noise present at any given frequency. When run on an unmodified development kit with no encoder filtering, the result looks like:

This shows that there are peaks in the noise at around 700Hz and 2kHz and after that the noise drops off rather rapidly. Then if we enable filtering at a few different bandwidths, you can see how the plot changes:

The filter bandwidths here varied from 5000 rps (~800Hz) down to 100rps (~15Hz). The filtered noise profiles follow what one would expect for each of the filters.

## Audible noise

To measure the combined effect of this and torque bandwidth on audible noise, I switched to a controller attached to an 8108 motor, as I’ve seen those tend to demonstrate more audible noise. The motor had a constant back and forth motion at 0.5Hz for 60s and the result was captured with a studio mic set about 1cm away from the motor. In each test, I ran moteus_tool --calibrate and selected only a torque bandwidth, letting it pick an appropriate encoder bandwidth. For the 100Hz torque bandwidth and above tests, the same position PID values were used, although they needed to be tweaked for stability at the lower bandwidths. I similarly plotted the power spectral density for several filtering values and for silence. The total signal strength measured in negative dB is noted in the label:

It is not shown here, but the unfiltered noise is about the same as the 400Hz one. Thus, the maximum improvement is around 6dB of audible noise. Filtering at 100Hz gives most of that benefit for this motor, with slight improvements beyond that. Most of the audible energy is in the spectrum below 1kHz and there are several frequency bands where the encoder does not appear to be the dominant source of audible noise, as all options are equivalent in those.

For comparison purposes, the ODrive firmware defaults to filtering both the torque and encoder at around 160Hz (1000rps).

## moteus_tool integration

As of version 0.3.19, moteus_tool has support for configuring the encoder bandwidth during calibration. By default, it will select a value appropriate for the selected torque bandwidth, or you can specify it manually with --encoder-bw-hz.

# Auto-tuning current control loops

Since the moteus controller was first released, it has implemented a two-stage controller. The outer loop is a combined position/velocity/torque PID controller, which takes as an input a position trajectory, and outputs a desired torque. The inner loops accepts this torque, and uses a PI controller to generate the Q phase voltage necessary to achieve that torque.

Until now, the constants for that PI controller were left as an exercise for the reader. i.e. there were some semi-sensible defaults, but the end-user ultimately had to manually select those constants to achieve a given torque bandwidth. That isn’t too much of a problem for a sophisticated user, but for the rest of us, it is hard to know how to go from a desired torque bandwidth to reasonable PI gains.

Now, I’ve finally decided to tackle this problem! There are two phases, the first being how to measure the inductance of a given motor, which is last major motor parameter to be automatically measured. The second is how to generate a controller using the resistance and inductance which has a desired bandwidth.

## Measuring inductance

An inductor is an energy storage device where the rate of change of current is directly proportional to the voltage across its terminals. Most (probably all?) electrical motors act as inductors in various forms. The hobby outrunners typically driven by moteus controllers, surface permanent synchronous magnet motors, can be treated as two equal valued inductors across the imaginary D and Q axes.

Given an ideal inductor, an easy way to measure that inductance is to apply a constant voltage and see how fast the current rises. That’s roughly the approach I took with moteus, but instead of a fixed voltage, it applies a square wave voltage centered about 0 across the D axis. That way the motor doesn’t move, the net current is 0 over time, and the average rate of change can be integrated over many cycles for much higher accuracy.

This “square wave” mode is implemented as a new control mode in moteus, primarily intended to be invoked by moteus_tool during the calibration process. You can specify the half-period of the square wave in control cycles, and the voltage to apply across the D axis. Here’s a plot of the current, as reported by a moteus controller over its debug DAC connection during a test on an mj5208:

This shows that yes, the current is roughly triangular for the input square wave.

## Designing a current controller

Given this newly measured inductance, and the phase resistance that the calibration process already measured, we have sufficient information to design a PI controller which achieves a desired bandwidth response.

For a PI controller, a simple second order approximation of the closed loop transfer function can be written as:

$I(s)=\frac{K_p s + K_i}{L s^2 + (R+K_p) s + K_i}$

Where L is the D/Q phase inductance, R is the phase resistance, and Kp and Ki are the PI gains. Taking that, if we constrain:

$K_i = K_p \frac{R}{L}$

We can then substitute this into the original transform, which results in a first order system:

$I(s)=\frac{K_p}{L} \frac{1}{s + \frac{K_p}{L}}$

This system has a 3db frequency at the sole pole:

$w_{3db} = \frac{K_p}{L}$

Which then allows us to fully determine all the remaining parameters:

$K_p = w_{3db} L$
$K_i = w_{3db} R$

Finally, when we double check our results, the “rise time” for a first order system (roughly the time it takes to go from 10% to 90%) can be described as:

$t_{rise} = \frac{0.35}{BW_{hz}}$

A motor like the mj5208 can be approximated as a phase resistance of 0.04 ohm, with a D/Q phase inductance of 25e-6 H. For a 3dB bandwidth of 1000 radians per second (~160Hz), the gains measured in radians per second would be:

$K_p=0.025$
$K_i = 40.0$

To set these on the controller, we would use the following parameters:

servo.pid_dq.kp=0.025
servo.pid_dq.ki=40.0

## Empirical testing

To make sure these make sense, I used this process for a few different bandwidths on a few different motors. For each, I used an oscilloscope to measure the rise time of the current waveform in response to a step input of a relatively small 4A command. The shape of that curve lets us be pretty confident that the damping ratio is correct, and the total rise time of the curve gives a good measure of the overall bandwidth that was achieved.

Across the motors I tested, the resistance varied from 35-65 milliohm, with inductances varying from 9uH to 33uH. The grid shows that the bandwidth worked out pretty close to the target in all those cases.

## moteus_tool integration

Now that the math is out of the way, you don’t need to worry about it at all, because as of version 0.3.17 of moteus_tool (with version 2021-04-09 of the firmware), this whole process is automated. When you run the calibration step, you can just specify the desired 3db bandwidth on the command line of moteus_tool and it will calculate and set the parameters for you. By default it will select 100Hz bandwidth.

python3 -m moteus.moteus_tool -t 1 --calibrate --cal-bw-hz 100

## Video

If you want to see this content explained more verbosely, here’s a video that covers most of it!

# moteus position anti-windup

The moteus controller uses a somewhat unique integrated position / velocity / torque controller with per-command configurable proportional and derivative gains. Through various combinations of these settings, it can emulate many different types of controllers, but one that it has struggled with until now was a pure velocity controller.

It has been minimally possible to use moteus as a purely velocity controlled since wraparound support was implemented, but that came with a caveat. Either the proportional term needed to be set to 0, in which case velocity tracking performance was poor, or if the proportional term was non-zero, an external torque would cause the position to drift arbitrarily far from the target position. Then if the external torque were released, the controller would “catch up” for all the lost ground, moving very rapidly.

Now, however, as of release 2021-03-05, an optional configurable parameter has been added to the moteus firmware which enables the “servo.max_position_slip” option. When this is finite, it acts as an anti-windup term on the position tracker, keeping it from getting far out of line. Tuning this lets you control how hard the controller tries to track velocity in the face of external disturbance, and how much catching up it will do when that external disturbance is relaxed.

This wasn’t conceptually hard to implement, but needed careful construction to interact properly with the existing stop position and position bounds that the firmware implements.

Here’s a video demonstrating the problem, and how the new configurable lets you resolve it:

# Stable gait sequencing

In the last post, I described the newer gait engine which takes a desired command and produces a set of gait parameters. At that point, the gait engine needs to implement those gait parameters in a way that is stable with respect to disturbances and keeps the two legs properly out of phase with one another.

The gait variables that the gait selection procedure emits are as follows, each “leg” is actually a pair of legs.

• swing time: This is the amount of time between when a leg lifts off the ground, and when it lands again.
• one leg time: This is the amount of time spent with only a single leg on the ground.
• two leg time: The amount of time with both legs on the ground.
• flight time: The amount of time with no legs on the ground.

I’ve chosen to implement this for now with two controllable variables, when to pick up the next leg to begin its swing phase and how far out to place the foot when landing. When in flight, the swing time is fixed to be exactly what the gait engine selected. When on the ground, the leg position and velocity are fixed in the world frame to match the current command. This results in the other three parameters, one leg, two leg, and flight time, being approximated as best as possible.

## When to lift

When there is no flight time, then the heuristic for when to lift the leg is identical to that described in “Balancing gait in 2D“. The trailing leg is lifted when the opposing leg is half of a swing time away from crossing the balance point.

When there is a flight time present, we have to add a corrective term to that heuristic, otherwise the two legs can lose their phase synchronization. If the correct out of phase relationship is being held, then the leg will be lifted when the alternate leg is in flight and has the “flight time” remaining in its swing cycle. The correction for that leg is calculated as a difference in time around that point such that the correction is positive before that point and negative after scaling in a linear manner. This has the desired property that the two legs are kept out of phase, without enforcing a strict leg cycle time.

## Where to place

Once again, when there is no flight time, the placement location is the same as before. When there is a flight time, then the placement location is even simpler. Now it is just a position forward of the balance point by one half of the “one leg time”. This results in the machine spending roughly half of its time on each side of the balance point.

## Further work

I’ve got one further enhancement to this technique to describe, at which point I think I’ll have basically exhausted its promise. Further advances will likely have to come from using an optimization based controller rather than a heuristic driven approach.

# Higher speed gait formulation

As hinted in my earlier video I’ve been working towards some higher speed gaits with the quad A1. To accomplish that, I had to restructure the gait sequencing logic to permit changing cycle times and allow flight phases.

For now, I’ve tentatively broken down the trot gait into 5 regimes, based on how fast the machine is moving:

1. At the slowest speeds, the flight legs swing through a step in the configured maximum flight time. The interval between flight times is fixed at a configured maximum. Here the speed is determined by how far the flight legs move.
2. Once the flight legs are moving through their maximum allowed distance, then the amount of time spent with both legs on the ground is reduced in order to increase speed.
3. At the point when both legs are not on the ground at the same time, then there begins to be a flight phase. Increasing the length of the flight phase increases the speed.
4. When the flight phase reaches a configured maximum, then the swing time is decreased until it reaches a configured minimum.
5. When the swing time is at a configured minimum, the flight time is at a configured maximum, and the legs are moving through their maximum range, then the machine is moving at its maximum speed.

Depending upon the current commanded rotation rate and translation velocity, the distance available for the legs to travel through may change. This uses the same mechanism from the step selection technique to determine the maximum distance at each update cycle, then selects which of the above regimes is active based on the commanded speed.

For a given maximum distance the legs can move through, that results in key gait parameters changing with speed as in the plot below:

Next up I’ll cover the heuristics used to implement any given set of gait parameters in a stable manner.

# Trotting with a flight phase

Here’s another short video only update, I’ve been experimenting with flight phases on the quad A1. With the gait formulation as I have it now, it isn’t terribly stable, but with some coaxing videos are possible:

# Unlimited rotations for moteus

The moteus controller has always supported multiple turns when counting positions. It has a one-revolution magnetic encoder built in, but after turn on, it keeps track of how many turns have occurred. However, if you’ve followed previous moteus tutorials, you have probably noticed a persistent caveat that for accurate control, the position of the output shaft needs to stay within a hundred revolutions of 0.0 or so. Now, I’ll describe why that was, and what I’ve done to remove the limitation, allowing unlimited rotations!

## Background

The moteus controller uses a somewhat unique integrated position / velocity / torque control loop. This formulation gives a couple of advantages: First, there is no bandwidth loss due to having a cascaded position and velocity controller. Second, when driven by a higher level controller, it can seamlessly switch between position, velocity, and torque control, or any combination of them without having to manage mode transitions.

The command consists of the following values, all as 32 bit floating point values (optionally upscaled from integer values using the register protocol).

• Desired Position
• Desired Velocity
• Feedforward Torque
• kp scale
• kd scale
• Maximum Torque

The control loop measures two quantities as input, the “current position” and the “current velocity”. The position is measured as a 32 bit signed integer, where one revolution of the magnetic encoder equals 65536 counts. The velocity is numerically differentiated across the most recent 6.4ms of movement.

There are two internal state variables as well: One is the “target position”. This captures the most recent position command, and is advanced by the velocity command at the full control rate. The other is the integrative term of the PID controller. Both of these are stored as 32 bit floating point values.

## The problem

This structure poses a few inherent limitations. One, being that as the control position is sent as a floating point value, the resolution available for positioning decreases as you get further from 0. That probably isn’t a big limitation, as there aren’t many applications where you want to have both absolute positions and also unlimited revolutions.

The bigger limitation is in the “target position” internal state variable. It needs to be updated to take into account the current velocity command at every control cycle, or 1/40000 of a second. For a commanded speed of 0.01 revolutions per second, this incremental update is only 2.5e-7 of a revolution. Given that 32 bit floating point values only have roughly 7 decimal digits of mantissa available to them, you don’t have to get far beyond 0 before an update that small doesn’t even change the value at all.

The command format also has an option, such that if the command position is set to a floating point NaN value, it will “capture” the current position. This can be used to command velocity-only control with an implicit integrative term, or when combined with a stop position to move to a target at a fixed velocity. However, since “capturing” stores the value as a floating point value, significant precision can be lost. This was only a problem at larger position values, but at the maximum position before wraparound, the available capture resolution was measured in multiple degrees.

## The resolution

The resolution was relatively straightforward. Instead of storing the “target position” as a floating point value, it is now stored as a 64 bit integer measured in 1/(2**32) of a magnetic encoder revolution. This gives sufficient precision to represent velocities as small as 0.0001Hz (0.036 dps) uniformly at all positions, while still having more absolute range than the measured current position value. The final PID controller is then expressed relative to the target position. This lets it still operate in floating point coordinates, but with no worry about large artifacts due to a position offset.

The only other implementation hurdle was making it run fast enough. Largely that revolved around ensuring there was never a need to convert between 64 bit integers and floating point values, which is relatively slow on the STM32G4.

## The result

With this fix in place, it is possible to operate the controller safely at high velocities for arbitrary periods of time. Even when the “current position” value wraps around from positive to negative! Also, low speed control works just as well at any position offset. When operating in those “continuous rotation” applications, the user should just be careful about if the “desired position” field of the command should be set. Largely, it should be left as NaN for when used in continuous rotation applications.

Here’s a video showing high speed wraparound and low speed at arbitrary offsets.

# New “stay within” control mode for moteus

At the request of @nichols in discord, I’ve recently implemented a new control mode in the moteus controller, “stay within”. In this mode, as long as the controller is inside the currently commanded bounds, only a feedforward torque is commanded. When either of the optional lower or upper bound is violated, the normal PID controller is used to force the position back to the bound.

Here’s a quick video demo:

Note that this could have been roughly accomplished in a couple of ways by a higher level controller — either by monitoring the position and commanding zero kp/kd scales when inside the boundary, or just solely commanding feedforward torques based on position sensing. However, this approach lets the control run at the full 40kHz of the moteus controller, which results in much smoother operation at the boundary condition.

# Optimizing moteus FET drive strength

The moteus controller uses a DRV8323 smart driver IC to drive the power MOSFETs as well as provide various safety functions. One of the capabilities it has which has so far been unexplored in moteus is its ability to control the drive strength and dead time through software configuration.

In a switching power supply or switching motor inverter, MOSFETs are arranged in a half bridge configuration. Depending upon the type of converter, one or more half bridges are used (3 phase inverters like moteus use 3 of them). Each “half bridge” has two MOSFETs, one connected between positive power and the output terminal, and the other connected between the output terminal and ground.

Power MOSFETs typically have relatively large gate capacitance, so to change their state quickly requires a lot of current. Additionally, you never want both MOSFETs conducting at the same time, otherwise current would flow straight from the supply to ground, called “shoot through”. Thus the driver has a configurable “dead time” which enforces that both are off for at least that long when switching (currents flow through the body diodes of the MOSFETs during this state).

## Optimization

Selecting these parameters is a balancing act. If the drive current is too low, the MOSFETs take a long time to turn on and off, which means they spend more time in a high resistance state. At some point however, higher drive currents don’t make the MOSFETs switch any faster, and just burn more power in the driver without any benefit. Similarly for the dead time, if it is too low this will result in shoot through, and if too high, it will result in current flowing through the body diodes for longer, which is much less efficient.

Until now, I hadn’t done any real optimization of these parameters, aside from ensuring the system was functional within a safety margin. In advance of some other to be announced developments, I decided to take another look.

To make a test, I set up a moteus controller with a test motor, but set up so that there was no thermal connection between the motor and the controller, and that the controller was not heatsinked at all. That would allow me to more easily determine how much heat was coming from the controller itself. Then, for various supply voltages, I commanded a fixed D phase current with just enough Q phase voltage so that the motor gently spun around. This ensured that all 3 of the half bridges were used equally. Then, I waited until things had reached thermal equilibrium, and used my DIY thermal board inspector, to measure the temperature of the motor windings, FETs, and the DRV8323.

With that test methodology in hand, I was able to search and locate the optimal drive strength, and discovered that I can use the smallest available dead time with no problems.

So, a nice win, especially at higher input voltages. The updated settings are in git master now, and will soon be in a new release.

# Cartesian leg PD controller

As I am working to improve the gaits of the mjbots quad A1, one aspect I’ve wanted to tackle for a long time is improving the compliance characteristics of the whole robot. Here’s a small step in that direction.

## Existing compliance strategy

The quad A1 uses qdd100 servos for each of its joints. The “qdd” in qdd100 stands for “quasi direct drive”. In a quasi direct drive actuator, a low gearing ratio is used, typically less than 10 to 1, which minimizes the amount of backlash and reflected inertia as observed at the output. Then, high rate electronic control of torque in the servo based on current and position feedback allows for dynamic manipulation of the spring and dampening of the resulting system.

Another option is a series elastic actuator, which uses a traditional high gear reduction servo with a mechanical spring or elastic mechanism inline with the load. Sometimes a separate motorized actuation mechanism can be used to vary the damping properties of the elastic element. This is in principle similar to the quasi direct drive approach, but suffers from a limited overall control bandwidth. Despite being “springy”, QDD servos are still able to have a very high effective mechanical control bandwidth, on the order of hundreds of hertz.

For the quad A1 to date, the compliance it exhibits is largely due to the qdd100’s internal control algorithms, and to a very minor extent, flexing in the mechanical structures of the quad A1 itself. This does work, and gives decent results.

## Limitations

The biggest limitation of solely using this approach, is that since the compliance is performed at the joint level, it has no knowledge of the current 3d configuration of the leg. The resulting compliance in 3D space is highly non-linear and depends upon where in configuration space the leg is at that point in time. For instance, if the back legs are configured to have the knee very bent, but the front legs are not, then the back knee needs a much larger restorative torque per unit rotation to have the same linear restorative force at the tip of the leg.

That results in artifacts like shown in the video at the bottom. When the robot falls with the legs not in an identical configuration, the robot ends up pitching or rolling depending upon how the compliance interacts with the current leg geometry.

## A “fix”

In my original designs for the moteus controller, I had left a high rate “inter-leg” bus option in the design, where each controller could exchange IK information at the full control rate, so that all compliance could be performed in the 3D space, rather than in joint space. However, as the design progressed, and I failed to implement it, I dropped that capability to simplify and reduce costs.

Here, I ended up implementing something purely in software which doesn’t have the same level of performance as that system would have, but also doesn’t require additional dedicated high rate communication transceivers on every servo control board. The 3D PD controller is just run on the raspberry pi at the regular control update rate (400Hz currently). That makes the control flow look like this:

## Results

While this solution isn’t perfect, it does give better results in many scenarios. I applied some disturbances to the robot with either solely joint level controllers, or joint plus XYZ controllers. For the two cases, I tried to tune the controllers to a similar level of stiffness and damping to make the comparison as fair as possible. Walking is generally improved as well, even with just a constant compliance throughout the gait cycle.