Implementation of driftless pitch

Official forum for open source FreePIE discussion and development.
Post Reply
konstantin_lozev
Cross Eyed!
Posts: 192
Joined: Fri Jul 04, 2014 1:43 am

Implementation of driftless pitch

Post by konstantin_lozev »

Happy new year to all :)
I had mentioned on a few occasions my troubles with drift using GlovePIE (and FreePIE). I just managed to write and test a driftless pitch script for GlovePIE. I thought I'd post it here if it could serve as an inspiration for enhancing the current fusion implemented in FreePIE (on my PC FreePIE and GlovePIE had a very significant drift). I think it might also be a good idea if as a first stage at least the gyro readings are exposed to FreePIE users (it seems now only a Pitch value is available and not the original PitchSpeed value that the gyro measures, forgive me if I am wrong). I tried to put as many explanations to the script as I could think of, but forgive me for any bad scripting techniques or cumbersome appropaches, I am only an enthusiast and not a professional.
The technique employed might seem too trivial for most of you, but feel free to suggest improvements. Basically, I have the gyro readings over time (with a Delta(timestamp)) accumulate in a variable and then every 0.05 radians of gyro-measured movement I trigger alignment to unfiltered accelerometer readings for the pitch based on the G measurements over the z and y axes. Pitch based on the accelerometer readings appeared trickier than I thought initially since there is a loooot of noise around 90 degrees along the z and x axes, so I had to switch from aSIN to aCOS. I know that it's usually aTAN that is used, but having the two accelerometer readings of z and y as an argument increased the noise levels twice. Same is the reason why I did not implement a compensation for roll (by having the combined x and y vectors or 1-z^2), it simply suffers from too much noise.
The script is highly customizable, so anyone can experiment with the variables, if he wants to.
Here is the link to the script
https://drive.google.com/file/d/0B_7L0f ... sp=sharing
And a collection of all earlier versions of the script, including some alternative methods that I tried, but thought they gave worse results.
https://drive.google.com/file/d/0B_7L0f ... sp=sharing
I also made a short and simple video of how the alignment works with the built in GlovePIE 3d simple rendering and a short Crysis 2 demo:
http://youtu.be/jPP8XQRqlLA
My interest now is how to implement this in FreePIE.
CyberVillain
Petrif-Eyed
Posts: 2166
Joined: Mon Jun 22, 2009 8:36 am
Location: Stockholm, Sweden

Re: Implementation of driftless pitch

Post by CyberVillain »

It depends on which hardware you are using we got gyro readings for android and wiimote

Code: Select all

android[0].raw.gx
wiimote[0].motionplus.pitch_left
NeoTokyo_Nori
Cross Eyed!
Posts: 144
Joined: Wed Jul 16, 2014 10:29 am
Location: Tokyo, Japan

Re: Implementation of driftless pitch

Post by NeoTokyo_Nori »

Happy new year.

@konstantin_lozev I checked the video, and it looks like you managed to get the drift under control, which is great. Your glovePIE script looks long, and I havent used glovePIE for a while, so I am not sure if I will be able to understand it. But it would be great to be able to port this over to FreePIE.

cheers.
konstantin_lozev
Cross Eyed!
Posts: 192
Joined: Fri Jul 04, 2014 1:43 am

Re: Implementation of driftless pitch

Post by konstantin_lozev »

NeoTokyo_Nori wrote:Happy new year.

@konstantin_lozev I checked the video, and it looks like you managed to get the drift under control, which is great. Your glovePIE script looks long, and I havent used glovePIE for a while, so I am not sure if I will be able to understand it. But it would be great to be able to port this over to FreePIE.

cheers.
Hi, Neo, Happy New Year to you too :). Yeah, I am pretty sure the pitch drift is safely under control within the cage of the accelerometer data, if I can use the metaphor, which explains quite well what it does actually :)
The part of the video that is with the rectangle (cube looked from the side) may seem choppy, but it only affects the rendering of the cube and is due to the fact that Crysis 2 runs in the background and not because the alignment makes the mouse movement choppy (which is evident from the in-game part after that). You are right, the script is long, but it's mostly window dressing, the core is quite simple and small actually. I cut the most relevant lines below and simplified them a bit, if you want to have a quick idea:
---------------------------------------
var.delta = (Removeunits(wiimote.PitchSpeed in radians per second)*Removeunits(Delta(timestamp)))// this is the variable that stores the changes to be made to the pitch at the end of the frame, it absorbs the effect of the gyro readinds here and later gets corrected if the alignment of gyro and accelerometer data is triggered for the particular frame
...
if abs(wiimote.gz)>1 { // this normalizes gz to 1
var.corrgz = sign(wiimote.gz)*1
} else {
var.corrgz = wiimote.gz
}

if abs(wiimote.gy)>1 { // this normalizes gy to 1
var.corrgy = sign(wiimote.gy)*1
} else {
var.corrgy = wiimote.gy
}
...
if (abs(var.corrgz)<=var.sincosswitch) { // this switches from aSIN to aCOS when > +-45 degrees. It appears that aSIN,aCOS gives least noise around +/- 90 degrees,
if (var.corrgy>0) { //this is the check that is meant to enable the correction of angles beyond 90 degreess
var.cumulpitchspeedacc = aSin(var.corrgz) in radians // var.cumulpitchspeedacc = aSin(var.corrgz) in radians is the acceelerometer-measured pitch for pitch below +/-90
} else {
var.cumulpitchspeedacc = sign(var.corrgz)*pi()-aSin(var.corrgz) in radians // this is the correction for andles beyond 90 degrees
}
} else {
var.cumulpitchspeedacc = sign(var.corrgz)*aCos(var.corrgy) in radians // sign(var.corrgz)*aCos(var.corrgy) in radians is used for angles > +/- 45 degrees
}
...
var.deltaaccgyro = var.cumulpitchspeedacc-var.corrcumulpitchspeedgyro // this measures at each frame the difference between the accelerometer and gyro readings
var.deltacumulpitchspeedgyro = var.oldcumulpitchspeedgyro - var.cumulpitchspeedgyro // this measures the deviation of the gyro measured pitch since the last point where it was aligned to the accelerometer data
...
if (abs(var.deltacumulpitchspeedgyro)>var.alignmentmarker) { // this sets the trigger for the alignment section of the script
var.startcorr = true
}
...
if (var.startcorr = true) { // this activates the script that gradually alignes gyro to accelerometer data when the gyro readings have moved beyond the marker since the last alignment.
if (Sin(abs(var.deltaaccgyro)) > var.deltatolerance) { // this stops the alignment after the difference between the accelerometer and gyro readings is decreased under var.deltatolerance
var.corrstep = abs(var.deltaaccgyro/var.corrstepnumber) // this sets dynamically each frame the extent of the correction/alignment towards the accelerometer data. The bigger the difference between gyro and accelerometer readings var.deltaaccgyro, the bigger the alignment step per frame (which means the alignment is actually decelerating over time, which is what we want). With the var.corrstepnumber you can tweak yourself that deceleration further. The default is 5 (float, so can also be e.g. 4.7), if you want faster alignment, set it below 5, of you want smooter, but laggier alignment, set it above 5.
var.delta += sign(var.deltaaccgyro)*var.corrstep
} else {
var.oldcumulpitchspeedgyro = var.cumulpitchspeedgyro // this resets the dybamic marker after the algnment has finished
var.startcorr = false // this switches off the aliggnment untill the next marker
}
}
...
var.corrcumulpitchspeedgyro += var.delta // this corrects/aligns the gyro readings to the accelerometer reading by assigning the corrected delta to the final pitch variable
...
if (odd(var.switcher) { // The check enables the switching of the wiiimote tracking
var.fake -= Removeunits(var.delta)*var.sensitivity // var.fake is a value that retains the real data that is transferred to mousedurectinput in order to assign it to mousedirectinpit each frame, thus avoiding accumulation of rounding
Mouse.DirectinputY = var.fake //this assigns the corrected updated non-rounded pitch (var.fake) to the mouse. The MousedirectInput accepts only whole numbers, so it rounds out this assignment. Therefore, the use of the var.fake is necessary
} else {
var.fake = Mouse.DirectinputY // resets the difference when the script is switched off with the home button (changes var.switcher)
}
---------------------------------------------
As you can see, there are 5 components to it:
1. Reading and storing the gyro data into var. delta
2. Converting the accelerometer data into pitch using aSIN and aCOS to obtain var.cumulpitchspeedacc
3. Triggering alignment to accelerometer data after the gyro has detected change in pitch beyond 0.05 radians (abs(var.deltacumulpitchspeedgyro)>var.alignmentmarker)
4. Gradual decelerating correction of gyro-measures pitch towards accelerometer-measured pitch (var.delta += sign(var.deltaaccgyro)*var.corrstep)
5. Accumulation of var.delta to a fake mouse variable var.fake and then assignment to the Mouse.DirectinputY. This additional step with the fake mouse variable is necessary because Mouse.DirectinputY accepts only whole numbers (mickeys), so it disregards the remainder at each frame, and that remainder accumulates over time leading to unnecessary drift, which is completely avoided by accumulating var.delta in the fake mouse variable. I think the example FreePIE mouse script may suffer from the same issue (although I have not tested it), which could be solved the same way.
Now, on my plans for conversion to FreePIE - my major hurdle is Python since I am only an enthusiast, so I never actually learned any Python (GlovePIE was more accessible to me since I gobbled a lot into Adobe Atmosphere, which used JavaScript, as well as some DHTML 15 years ago). So the plan is I learn first Python... I wish there were Google Translate for code :)
CyberVillain
Petrif-Eyed
Posts: 2166
Joined: Mon Jun 22, 2009 8:36 am
Location: Stockholm, Sweden

Re: Implementation of driftless pitch

Post by CyberVillain »

FreePIE has that built into the mouse plugin.
konstantin_lozev
Cross Eyed!
Posts: 192
Joined: Fri Jul 04, 2014 1:43 am

Re: Implementation of driftless pitch

Post by konstantin_lozev »

CyberVillain wrote:FreePIE has that built into the mouse plugin.
Great, one less thing to worry about when trying to port :)
CyberVillain
Petrif-Eyed
Posts: 2166
Joined: Mon Jun 22, 2009 8:36 am
Location: Stockholm, Sweden

Re: Implementation of driftless pitch

Post by CyberVillain »

konstantin_lozev wrote:
CyberVillain wrote:FreePIE has that built into the mouse plugin.
Great, one less thing to worry about when trying to port :)
Yupp :D

Look here for python math lib

https://docs.python.org/2/library/math.html
konstantin_lozev
Cross Eyed!
Posts: 192
Joined: Fri Jul 04, 2014 1:43 am

Re: Implementation of driftless pitch

Post by konstantin_lozev »

Thanks, can you please tell what is the default fusion algorithm for the wiimote besides the Machony one?
CyberVillain
Petrif-Eyed
Posts: 2166
Joined: Mon Jun 22, 2009 8:36 am
Location: Stockholm, Sweden

Re: Implementation of driftless pitch

Post by CyberVillain »

Just integrating the gyro
konstantin_lozev
Cross Eyed!
Posts: 192
Joined: Fri Jul 04, 2014 1:43 am

Re: Implementation of driftless pitch

Post by konstantin_lozev »

CyberVillain wrote:Just integrating the gyro
Ok, thanks, I have been reading of fusion algorithms lately and they all advise this http://www.x-io.co.uk/open-source-imu-a ... lgorithms/
It's a bit beyond me, but I think I would not need full understanding on how and why it works as long as I can translate the sample code for our purposes.
CyberVillain
Petrif-Eyed
Posts: 2166
Joined: Mon Jun 22, 2009 8:36 am
Location: Stockholm, Sweden

Re: Implementation of driftless pitch

Post by CyberVillain »

Mahony is taken from that exact code

https://github.com/xioTechnologies/Open ... onyAHRS.cs
konstantin_lozev
Cross Eyed!
Posts: 192
Joined: Fri Jul 04, 2014 1:43 am

Re: Implementation of driftless pitch

Post by konstantin_lozev »

Hm, I wonder then why I get such weird results using the Mahony fusion... The correction applied per frame seems just too small so I end up seeing the cursor get dragged to the accelerometer estimated orientation gradually for 1 sec after I lay the wiimote still. I am at p.5 of Magwick's paper and I am comfortable with the quaternion and its representation (not really difficult once I imagined it as an extension of a 2d case).
It was also weird to see magnetometer related code in our case, but I also noted that the algorithm prevents NaN returns anyway.
EDIT: I think I got it. If you look at page 29 of Magwick's paper, he proposed an IMU only code which is very easy to understand and corresponds to the easier parts of his paper. I may try to implement this code in GlovePIE and see the results.
CyberVillain
Petrif-Eyed
Posts: 2166
Joined: Mon Jun 22, 2009 8:36 am
Location: Stockholm, Sweden

Re: Implementation of driftless pitch

Post by CyberVillain »

You can look at the wiimote fusion code here

https://github.com/AndersMalmgren/FreeP ... usFuser.cs

Try replace it with Magwick or trim the values, for Mahony you can play around with Kp and Ki values and for Madgwick you can play with Beta values

Please let me know if you suceed in getting any better fusion and we can incorporate it into FreePIE
konstantin_lozev
Cross Eyed!
Posts: 192
Joined: Fri Jul 04, 2014 1:43 am

Re: Implementation of driftless pitch

Post by konstantin_lozev »

thanks, will do :)
Professor_Hoover
One Eyed Hopeful
Posts: 17
Joined: Sun Sep 28, 2014 11:03 pm

Re: Implementation of driftless pitch

Post by Professor_Hoover »

Was this solution ever properly ported to FreePIE? I'd like to try it out so I might try porting it soon if nobody else has.
konstantin_lozev
Cross Eyed!
Posts: 192
Joined: Fri Jul 04, 2014 1:43 am

Re: Implementation of driftless pitch

Post by konstantin_lozev »

Professor_Hoover wrote:Was this solution ever properly ported to FreePIE? I'd like to try it out so I might try porting it soon if nobody else has.
Do you refer to my GlovePIE script or to simplifying the Magwick's implementation in the FreePIE code?
CyberVillain
Petrif-Eyed
Posts: 2166
Joined: Mon Jun 22, 2009 8:36 am
Location: Stockholm, Sweden

Re: Implementation of driftless pitch

Post by CyberVillain »

Well porting is not the right term since the code is in C#, but If we got it to run nicely with for example raw data from an Android? No it drifts pretty much, probably more than Googles own fusion. Wiimote drifts even more. IF you want to give it a shot I can provide you with help on how to set FreePIE up in VS2013 Express and were to find the fusion logic etc
karoloydi
One Eyed Hopeful
Posts: 2
Joined: Tue Feb 10, 2015 4:10 pm

Re: Implementation of driftless pitch

Post by karoloydi »

What if we had 2 wiimotes strapped together and one would correct each other?
konstantin_lozev
Cross Eyed!
Posts: 192
Joined: Fri Jul 04, 2014 1:43 am

Re: Implementation of driftless pitch

Post by konstantin_lozev »

CyberVillain wrote:Well porting is not the right term since the code is in C#, but If we got it to run nicely with for example raw data from an Android? No it drifts pretty much, probably more than Googles own fusion. Wiimote drifts even more. IF you want to give it a shot I can provide you with help on how to set FreePIE up in VS2013 Express and were to find the fusion logic etc
Would it not he easier to try and implement the fusion in the actual script at first? This way it can be tweaked much more easily without needing a new build.
konstantin_lozev
Cross Eyed!
Posts: 192
Joined: Fri Jul 04, 2014 1:43 am

Re: Implementation of driftless pitch

Post by konstantin_lozev »

karoloydi wrote:What if we had 2 wiimotes strapped together and one would correct each other?
We do not need that for driftless pitch. Also, nothing guarantees they will cancel out.
CyberVillain
Petrif-Eyed
Posts: 2166
Joined: Mon Jun 22, 2009 8:36 am
Location: Stockholm, Sweden

Re: Implementation of driftless pitch

Post by CyberVillain »

Well, its offcourse a matter of taste but the code is there ready to use in c#
Professor_Hoover
One Eyed Hopeful
Posts: 17
Joined: Sun Sep 28, 2014 11:03 pm

Re: Implementation of driftless pitch

Post by Professor_Hoover »

I misunderstood how your code worked, I thought you had written a script for GlovePIE that would be fairly trivial to port compared to rewriting the fusions. I don't have the skills to do it properly, so I'll have to leave it to someone else for now. I might come back to it in a few months when I've finished the C# unit in my course.
konstantin_lozev
Cross Eyed!
Posts: 192
Joined: Fri Jul 04, 2014 1:43 am

Re: Implementation of driftless pitch

Post by konstantin_lozev »

Professor_Hoover wrote:I misunderstood how your code worked, I thought you had written a script for GlovePIE that would be fairly trivial to port compared to rewriting the fusions. I don't have the skills to do it properly, so I'll have to leave it to someone else for now. I might come back to it in a few months when I've finished the C# unit in my course.
Yeah, the topic somehow shifted. And yeah, my GlovePIE script is really fairly trivial, so should be easily ported if you knew your way around Python, unlike me. So, you can have a go at it, maybe first experiment with it in GlovePIE, then see how you can port it to Python. I think it would be best to put most of it in a function that takes the raw data and outputs the corrected data. I have not been able to define functions in GlovePIE for some reason.
You should know however that Magwick's approach is more sophisticated and involves dynamic "precooking" of the data before correcting and also applies to roll together with pitch, and even yaw if you had magnetometer. Mine is simply my idea of a very, very basic approach which works at least on my wiimote quite satisfactorily. You might need to tweak some of the variables if your wiimote's accelerometer noise is different.
If you need explanations on my script, I am ready to help.
Professor_Hoover
One Eyed Hopeful
Posts: 17
Joined: Sun Sep 28, 2014 11:03 pm

Re: Implementation of driftless pitch

Post by Professor_Hoover »

Did you need a lightbar for your setup? Your other posts suggest that my main issue at the moment is with not having a lightbar I can setup at my computer station. I really don't want to spend any money on this solution because I plan to buy an Oculus Rift when they release the consumer grade model, which would obviously make this setup obsolete.

Otherwise, why can't your script be redone for roll/yaw? The only drift I see in my current setup is the wiimote rotating left. Maybe I'll notice more once I get this sorted out, but I hope not. I'll have to go over Magwick's solution to find out if I actually understand it well enough to develop FreePIE related software.
konstantin_lozev
Cross Eyed!
Posts: 192
Joined: Fri Jul 04, 2014 1:43 am

Re: Implementation of driftless pitch

Post by konstantin_lozev »

Professor_Hoover wrote:Did you need a lightbar for your setup? Your other posts suggest that my main issue at the moment is with not having a lightbar I can setup at my computer station. I really don't want to spend any money on this solution because I plan to buy an Oculus Rift when they release the consumer grade model, which would obviously make this setup obsolete.

Otherwise, why can't your script be redone for roll/yaw? The only drift I see in my current setup is the wiimote rotating left. Maybe I'll notice more once I get this sorted out, but I hope not. I'll have to go over Magwick's solution to find out if I actually understand it well enough to develop FreePIE related software.
Hi, no, my script posted in this topic does not need IR bar. IR bar is ok for sitting in front of the TV and this is good for standing 360 degree or less. It is not possible to correct yaw on the wiimote by itself based only on the wiimote's IMU.
What is your setup?
Professor_Hoover
One Eyed Hopeful
Posts: 17
Joined: Sun Sep 28, 2014 11:03 pm

Re: Implementation of driftless pitch

Post by Professor_Hoover »

I have a wiimote strapped sideways to my headset (as in parallel with my shoulders). I then read the data with FreePIE and map the output to FreeTrack/TrackIR for games like Arma. At the moment, my wiimote causes my character's head to look to the left, which would be the wiimote rotating anticlockwise when viewed from above. Unfortunately, unless the wiimote does odd things with its axises then this is a yaw issue, which you've said can't be corrected.

EDIT: You can find the current state of my code here: http://www.mtbs3d.com/phpbb/viewtopic.p ... &start=160
konstantin_lozev
Cross Eyed!
Posts: 192
Joined: Fri Jul 04, 2014 1:43 am

Re: Implementation of driftless pitch

Post by konstantin_lozev »

Hi, is the yaw drift too big? Is it different in GlovePIE? See here, I had to correct the yaw drift at 8 min by pushing one button, now I moved it on the wiimote, but thepoint is itis not possible to correct yaw with the wiimote's IMU. https://www.youtube.com/watch?app=deskt ... fSFNb5QWqY
If you don't have an IR bar and you don't want to buy one, you can use 2 candles or 2 IR LEDs instead and that will give you fixed point of reference, so no drift.
For what you want to achieve, you can also switch things and make yourself a TrackIR clip with 3 LEDs and use the wiimote's IR camera at the front tip of the wiimote to track those with FreeTrack.
Well, it can be even easier for you to try FaceTrackNoIR.
Professor_Hoover
One Eyed Hopeful
Posts: 17
Joined: Sun Sep 28, 2014 11:03 pm

Re: Implementation of driftless pitch

Post by Professor_Hoover »

That looks pretty accurate. I'll get back to you once I've tried it out myself. Are you using that light on your head with another wiimote or just a webcam?

In my case, the yaw drift has my character's head twisted around in under 10 seconds.

I would use FaceTrack, but I don't own a webcam at the moment. I think I'll get a webcam eventually for other uses, but until then I wanted to try this. If worst comes to worst, I'll just haul the Wii into my computer room and use the lightbar because I can't think of anything else I own that outputs IR without being a fire hazard.
konstantin_lozev
Cross Eyed!
Posts: 192
Joined: Fri Jul 04, 2014 1:43 am

Re: Implementation of driftless pitch

Post by konstantin_lozev »

I would ask you to try GlovePIE with a simple script. If you install it, I will PM you a very simple script to test with it. And for the IR bar, you will not be sorry, no drift guaraneed :)
BTW, remote controls output IR light.
konstantin_lozev
Cross Eyed!
Posts: 192
Joined: Fri Jul 04, 2014 1:43 am

Re: Implementation of driftless pitch

Post by konstantin_lozev »

Professor_Hoover wrote:Are you using that light on your head with another wiimote or just a webcam?
Hi, I did not reply to that. This on my head is a bicycle LED and on the ceiling I put an Android phone and I use it as a wireless webcam. Which takes me to another suggestion for you. If you have an android phone, you can use it as a webcam with FaceTrackNoIR. It will be laggier than a normal webcam, but you can try for yourself. You can try DroidCam or IP Webcam from the Android market. I think one works with Freetrack and the other with FacetrackNoIR.
Post Reply

Return to “FreePIE”