thx for sharing the videos but first: I don't understand lejos code.
Then, 2nd: As far as I can see, you only use a gyro, but I need additionally accelerometers which are needed for horizontal straight, transversal, and vertical movement calculation, and a compass for filtering gyro drift/noise and accelerometer noise.
That means that I need:
a 3D Gyro
a 3D Accelerometer
a 3D Compass
current 3D position (x, y, z) in space (related to a start point),
current 3D speed in space (^x, ^y, ^z )
current 3D spatial orientation (roll, pitch, yaw)
That's a little bit more complex than I can see from your videos (please CMIIW).
Additionally, the DMP program must not require more than 1k on the NXT flash because the rest of the NXT memory is needed for my move control program, and much MCU calculation power is needed for several real-time PID regulators.
(For the moment it's only in 2D (my map and explore robot), but it is planned to use it in 3D for an autopilot for a quadrocopter or an aircraft, too.)
And the more the DMP-IMU can calculate on his own, the better for the NXT's real-time-performance.
NXC CHESS for NXT: http://www.mindstormsforum.de/viewtopic.php?f=70&t=6790
indispensable for NXC + EV3-C: easy + trouble-free network & remote control for rs485, Xbee, BT, USB!