Calibrating for Wii Remote with MotionPlus about micropython-fusion HOT 6 CLOSED

Maybe it's also not the algorithm I'm looking for anyway, since I'd want convergence to some pitch value in more like 0.1 seconds instead of the two that the README is saying. Raising GyroMeasError to 400 also seems to start giving values that are all over the place. But it might be good practice to see how it could be done with this library.

from micropython-fusion.

I've no experience with the Wii but it should work. However the behaviour you're getting is evidently wrong. You've probably already checked the following, but these are the thoughts that come to mind.

I assume your coordinate system conforms with this note. The Z axis accelerometer is easily identified by the 1G offset. You can check X and Y by rotating it as per that note and observing what happens to the offset. Gyro axes surely correspond unless the device is perverse; they can be checked similarly by orientating it on each orthogonal axis, running a loop that displays Gyro X, Y, Z repeatedly, and giving it a quick twist. Note that the gyro must produce degrees per second.

I haven't experimented with Beta very much: I've no idea whether 100ms convergence is feasible. I believe there are other more modern algorithms but I haven't studied them. I think Madgwick tends to prioritise accuracy over speed whereas most Wii applications require the opposite. Perhaps something can be done with Kalman filters or complementary filters?

from micropython-fusion.

The comments in the code describe a lot of the "whys" of what's there. It's a bit difficult for me to understand the coordinate systems, but leaving the accelerometer and gyro values in their default order didn't seem to produce correct results so I tried ordering them according to the order "yaw, pitch, and then roll" mentioned under section 5.1 Heading Pitch and Roll. I think it's better explained in the comment about what twisting the device each way does to the values. I also don't know what the positive direction should be for each case. The direction for pitch's axis I'm pretty sure is correct, though.

Note that the gyro must produce degrees per second.

That's something I tried to take into account. The raw angle_rates reported by the gyro are first calibrated to be near zero at rest (I'm not sure if that was necessary but still). Then they are scaled by a value that to my understanding should mean they will be in degrees per second, but of course my reasoning about it could be wrong.

There's no calibration to the accelerometer values. I tried to have them be 0 at rest, but then there were some divisions by zero so I concluded it's probably not what's wrong.

Perhaps something can be done with Kalman filters or complementary filters?

I started reading up on Kalman filters after noticing the Madgwick algorithm may not be suitable.

Oh, and I forgot to add an example of the kind of output this code gives, in case it points out anything. In this case the device has been at rest on the table for enough time for the values to more or less converge:

(122, 123, 147) (0.1, -0.7, 0.7)
Heading, Pitch, Roll: 0.000 -33.079 40.817

from micropython-fusion.

Hmm, this is interesting. I managed to get correct-looking numbers by adding a bias to the accelerometer values as well. Except, the value output as roll looks like that of pitch's and the value output for pitch looks like roll's. The while loop now looks like this:

while True:
    accel = (wiimote.state['acc'][0]-122, wiimote.state['acc'][1]-122, wiimote.state['acc'][2]-122)

    angle_rates = wiimote.state['motionplus']['angle_rate']
    gyro = (
        (angle_rates[0]-7945)/(4.0+wiimote.state['motionplus']['low_speed'][0]*16),
        (angle_rates[1]-8115)/(4.0+wiimote.state['motionplus']['low_speed'][1]*16),
        (angle_rates[2]-7964)/(4.0+wiimote.state['motionplus']['low_speed'][2]*16),
    )
    fuse.update_nomag(accel, gyro, time.time())
    if count % 5 == 0:
        print(accel, gyro)
        print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(fuse.heading, fuse.pitch, fuse.roll))
    time.sleep(0.02)
    count += 1

The output is now kind of like this:

(0, 1, 25) (7.4, -8.4, -0.6)
Heading, Pitch, Roll: 0.000 0.024 2.288

when level but

(1, -23, -1) (8.05, -7.75, -0.7)
Heading, Pitch, Roll: 0.000 -2.509 -91.775

when pointing upward and

Heading, Pitch, Roll: 0.000 -2.436 91.936
(1, 25, -1) (20.0, -4.0, 12.55)

when pointing downward. The pitch and roll are the wrong way around but otherwise it's looking good. It also reported facing backward as +-180, which the Kalman filter implementation I tried didn't.

However, with biases

accel = (wiimote.state['acc'][0]-122, wiimote.state['acc'][1]-123, wiimote.state['acc'][2]-122)

it crashes:

[mikko@localhost micropython-fusion]$ python3.7 fusiontest_wiimote.py 
Traceback (most recent call last):
  File "fusiontest_wiimote.py", line 51, in <module>
    fuse.update_nomag(accel, gyro, time.time())
  File "/mnt/USB/opinnot/SpecuSim/micropython-fusion/fusion.py", line 91, in update_nomag
    norm = 1 / sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4)    # normalise step magnitude
ZeroDivisionError: float division by zero
code 1

from micropython-fusion.

The units of the accelerometer don't matter as the algorithm normalises them, however typically I calibrate them to produce G as this is easy to understand. With the unit in its normal orientation the Z axis should then produce a value of 1, with the other axes nominally 0. By rotating the unit 90° about the X and Y axes in turn, you should see 1 or -1 on their accelerometer values.

As stated in the docs without a magnetometer heading values will be meaningless. It is physically impossible to deduce absolute heading from an accelerometer and a gyro.

from micropython-fusion.

Anyway, I feel like I can work with this, and it's much faster convergence, now that it's been calibrated, than before. Fast enough that it's at least a candidate for my purposes. I'm closing the issue.

Yeah, the heading will require a different tactic. The 180 thing was when I was pitching the remote so much it was upside down facing backward. Another thing I have access to is a "sensor bar" producing two infrared lights, which will give at least a starting point for the heading. I'm hoping that I can deduce the heading roughly from the last known position, the estimated roll over time and the gyro's data over time. But that's another topic to research.

from micropython-fusion.