Kalman Filter / DCM / Anything.....

th3jester

I am curious if anyone has a reasonable working filter for a 6DOF I2C IMU. I've tried using the Kalman filters from the OBEX and they weren't providing accurate enough approximations. I believe the reason was on my end for I am using an I2C interface when the Object is using the MCP A/D chip.

Details: Wii Nunchuck Accel with the Wii MotionPlus Gyro. I'm using a slightly modified object for those. All data is I2C.

I'm open to any ideas on filtering the data. I am not a math or stats genius, I am having difficulty understanding the procedures behind the filtering. This is for a Propeller Quadcopter. I will post the Quadcopter code once it is finished, right now its kind of a mess.

I have tried using the IMU-3000 from Invensense, yet no luck with that device. I was able to communicate and R/W a few registers but I wasn't able to get the DMP activated or any raw sensor values. So that is why I moved toward the Wii units.


Any help will be appreciated, thanks.
Matt

M. K. Borri

You may want to try the Akima Spline, especially if you have a sensor that isn't that precise but has good data rate.

JasonDorie

When you say they "weren't providing accurate enough approximations", did you alter the code at all?

I ask because the objects in the obex make assumptions about the input values being given them, such as the range of values, precision of the values, and the number of bits to a degree or radian. If any of those differ in your setup, you'd need to change the code accordingly or the results could be anywhere from slightly inaccurate to completely useless.

The most important one to be aware of is the unit conversion from your gyro input to degrees per second. A 12 bit ADC connected to a 300 deg/s rate gyro will have a different "bits per degree" conversion than the 14 bit I2C gyro at 600 deg/s on the WiiMotion.

th3jester

JasonDorie, you are correct. After posting my questions to the thread I finally decided to completely break apart the Wii object. The Wii Object is wrong and right. It will give bit values corresponding to the difference of change, there are two more math steps involved to get deg/sec.

Wii MotionPlus:
2.27mV/deg/sec with a 1.35 Vref
therefore 1.35Vref / 2.27mV/deg/sec = 594.7 deg/sec roughly 600deg/sec

raw centered values from the WMP are around 8192, so 8192 / 594.7deg/sec = 13.775 units/deg/sec

when reading the WMP take the raw centered value and subtract the current raw value, this will give you the difference of change in units.

If we have a raw centered value of 8192 and we read a current raw value of 8010 then:
8192 - 8010 = 182 units
therefore 182 units / 13.775 units/deg/sec = 13.21 deg/sec

The WMP has a fast mode and a slow mode which will change the deg/sec measurement. I plan on using only the slow mode to slow down the rate.

The Wii Nunchuck object is very similar in that it was only half completed with the math.

Today I will run my new values through the Kalman and see if I get a better result. I will make sure the Kalman constants relate to my measurements, although since I'm getting deg/sec and acceleration in g and in m/s^2 then I should be able to run the Kalman without more conditioning.

I've also realized my biggest problem is not understanding all the math in the Kalman Filter.

Thanks for you input.

''///////No offense to the person who wrote the Wii Objects/////////

Cluso99

Keep up the good work. Very enlightening as I hope to get to this shortly. I have the bits

th3jester

It is complete and working! ....well as far as I know, I could be thinking its right when in reality its completely wrong. But the filter reacts to changes in the orientation and does reduce drift and noise a bit.

Thank you again Jason, your input did help a lot. The main problem with the filter was that I wasn't converting to the right values. I've attached the objects I used minus the common ones (FullDuplexSerialPlus).

I tried to comment on the code where it was needed. The objects I've attached are combination's of several objects with original code as well. So I did my best.

To run this example program you will need a Wii MotionPlus and a Wii Nunchuck. The Nunchuck has to be connected to the MotionPlus (I2C pass-through). Also they are attached to the EEPROM (pins 28 and 29).

My next steps will be to weave this into my QuadCopter code and the PI control loop. The PI control loop works pretty well, if any is interested in it let me know, as the PID object on the OBEX is......very basic.

Also if you read through what I have posted and have any suggestions on formatting/architecture of my code please inform me as I am always willing to improve what I write.

KalmanFilterTestDisplay is the top Object file.

th3jester

A schematic will be created and posted later today.....need sleep first....and class.

JCee

Take a look at this project , he implemented DCM in spin. I dont have the hardware to test it but it may work for your needs.

I believe the kalman filter in obex was designed for a balancing robot where the orientation for only a single axis was being estimated. You may be able to get away with two independent filters for pitch and roll if your quad remains near level. But for aggressive quad maneuvers the balancing robot equations of motion dont hold up and the filter may diverge.

Good Luck

JoeBotics

th3jester wrote: »

I am curious if anyone has a reasonable working filter for a 6DOF I2C IMU. I've tried using the Kalman filters from the OBEX and they weren't providing accurate enough approximations. I believe the reason was on my end for I am using an I2C interface when the Object is using the MCP A/D chip.

Details: Wii Nunchuck Accel with the Wii MotionPlus Gyro. I'm using a slightly modified object for those. All data is I2C.

I'm open to any ideas on filtering the data. I am not a math or stats genius, I am having difficulty understanding the procedures behind the filtering. This is for a Propeller Quadcopter. I will post the Quadcopter code once it is finished, right now its kind of a mess.

I have tried using the IMU-3000 from Invensense, yet no luck with that device. I was able to communicate and R/W a few registers but I wasn't able to get the DMP activated or any raw sensor values. So that is why I moved toward the Wii units.


Any help will be appreciated, thanks.
Matt

Hi th3jester.... would you mind sharing the code used to R/W registers from the IMU-3000? I also have a module... and besides I2C recognizing the module I haven't been able to read/write anything to the IMU-3000... all your help is much appreciated !!.. thanks!!

th3jester

I no longer have my "test" code for the IMU-3000, I switched to using the Wii devices. But I was able to find a little snippet of the code. Understand, I spent several weeks trying to get IMU-3000 to give me something. I tried lots of different ways to communicate and receive values. I've used almost every single I2C object on the OBEX as well as trying to write my own basic I2C routines per the spec sheet on the IMU-3000. More information is available from Invensense if you register. You'll get a better manual/spec sheet and the source code they used for their demo board. Good luck and let me know if you figure anything out, cause I would still like to use that board if the DMP is working on it.

The code I have attached only shows how I wrote and read certain registers. I was never able to get gyro/accel measurements.

JoeBotics

th3jester wrote: »

I no longer have my "test" code for the IMU-3000, I switched to using the Wii devices. But I was able to find a little snippet of the code. Understand, I spent several weeks trying to get IMU-3000 to give me something. I tried lots of different ways to communicate and receive values. I've used almost every single I2C object on the OBEX as well as trying to write my own basic I2C routines per the spec sheet on the IMU-3000. More information is available from Invensense if you register. You'll get a better manual/spec sheet and the source code they used for their demo board. Good luck and let me know if you figure anything out, cause I would still like to use that board if the DMP is working on it.

The code I have attached only shows how I wrote and read certain registers. I was never able to get gyro/accel measurements.

Hi th3jester!.... thanks much for the info.... yeah... I registered and downloaded all the related IMU3000 stuff... including the demos (I also have the INV-ARM module to connect the IMU3000EVB to my PC).. I went through some of the Visual C++ 2005 code and I can see some routines used for receiving values via the MPL libraries... one strange issue I have is that when I use the i2cScan routine from the Basic I2C Driver.. the IMU3000 module appears to be at i2C address %1000_1110 and not at %110_1000 as described by your routines and the documentation... go figure!... and although I seem to be able to communicate with the IMU3000 to read registers... trying to modify registers does not change the values... I created some simple Spin code to dump the values of the registers... it is attached to this post... also... I am attaching a JPG with the results of my register dump... perhaps you can also run it and compare values??... we should join forces!!.. :-)

One more thing... I am using a Propstick USB... how did you connect the IMU3000 to your propeller?... besides SCL/SDA... I used the onboard Propstick regulator that generates 3.3v and I am feeding it directly to VLOGIC on the IMU3000EVB module.. the other alternatives (like feeding 3.3v - 5v on VCC_IN and selecting jumping JP1 - JP2 as EV0 didn't work for me... perhaps I fried the onboard IMU3000 regulator)... thanks again!

Cluso99

Nice work jester. Hopefully I will be getting a bit of time shortly. I have all the bits to build my quad apart from the metal frame. I have normal props but would like to use counter-rotating ones.
For my frame I have some round aluminium tube so I can twist the motors for standard props. BTW did you realise you can package all your files with proptools into a zip file (file archive)?