Simple Quaternion Library?

michaellangford99

Hello,

I was wondering if anyone knows of a Quaternion library for use in an IMU, or how to make one. In my current setup, my gyro will lose 90 degrees if I tilt it 90 in pitch, then roll it 90, and then put it back down, because it is just integrating the rate. This would not happen if a Quaternion was used. Even though I am Kalman filtering my gyro and accelerometer, I will still get some data loss for a few iterations. I am making a quadcopter, and though the rotations described would probably never happen, anything remotely like it could severely mess the flight up.

Heater.

I have a quaternion in Javascript here:
https://github.com/ZiCog/madgwick.js

Includes Madgwick's and Mahony's AHRS accel, gyro and compass fusion code in C and Javascript.

From my limited experiments it works quite well with out the compass inputs.

The C could be usable on the Prop as is, if it fits. Else you will have to port that to Spin.

Didn't someone already do this in Spin ?

JasonDorie

The DCM code I did a while back does the same thing, and that's in Spin.

It's not a quaternion, but it's easy enough to get the angles out using ArcSin.
http://forums.parallax.com/discussion/131022/propeller-dcm-now-with-source/p1

And there's also: https://github.com/parallaxinc/Flight-Controller

It uses a quaternion for the updates, then converts to a matrix to do the drift compensation. It's still in development, but it's flyable now. I'm hoping since it's open source, no one will mind me posting it early.

michaellangford99

Thanks guys! This looks useful! I will post the final code.

SRLM

I wrote a fun "pre-compiler" in Python that would take specially formatted Spin files and generate assembly based math coroutines. For example:

https://github.com/srlm-io/anzhelka/blob/master/software/spin/src/block_moment.spin

PUB Calculate
'One iteration of the calculations
'Should be called from a repeat loop...
'Writes object local variables, does not write to address
	
	fp.FInterpret(@MOMENT_BLOCK_INSTRUCTIONS)
	
{{AZM_MATH MOMENT_BLOCK
'q star:
q_1 = 0 - q_1
q_2 = 0 - q_2
q_3 = 0 - q_3
'Moment Block, first Quat Mul
q_tilde_0 = ((q_d_0*q_0) - (q_d_1*q_1)) - ((q_d_2*q_2) - (q_d_3*q_3))
q_tilde_1 = ((q_d_0*q_1) + (q_d_1*q_0)) + ((q_d_2*q_3) - (q_d_3*q_2))
q_tilde_2 = ((q_d_0*q_2) - (q_d_1*q_3)) + ((q_d_2*q_0) + (q_d_3*q_1))
q_tilde_3 = ((q_d_0*q_3) + (q_d_1*q_2)) - ((q_d_2*q_1) + (q_d_3*q_0))
alpha = 2 * (q_tilde_0 arc_c 0)
t_1 = (alpha / 2) sin 0
...

Resulted in:

	fp.AddSequence(MOMENT_BLOCK_INDEX, @MOMENT_BLOCK_Instructions)

	const_0 := float(0)
	const_1 := float(1)
	const_2 := float(2)'q star:

'------------
'' q_1 = 0 - q_1
	'q_1 = const_0 - q_1
	fp.AddInstruction(MOMENT_BLOCK_INDEX, fp#FPSub, @const_0, @q_1, @q_1)

'------------
'' q_2 = 0 - q_2
	'q_2 = const_0 - q_2
	fp.AddInstruction(MOMENT_BLOCK_INDEX, fp#FPSub, @const_0, @q_2, @q_2)

'------------
'' q_3 = 0 - q_3
	'q_3 = const_0 - q_3
	fp.AddInstruction(MOMENT_BLOCK_INDEX, fp#FPSub, @const_0, @q_3, @q_3)
'Moment Block, first Quat Mul

'------------
'' q_tilde_0 = ((q_d_0*q_0) - (q_d_1*q_1)) - ((q_d_2*q_2) - (q_d_3*q_3))
	'azm_temp_0 = q_d_0 * q_0
	fp.AddInstruction(MOMENT_BLOCK_INDEX, fp#FPMul, @q_d_0, @q_0, @azm_temp_0)
	'azm_temp_1 = q_d_1 * q_1
	fp.AddInstruction(MOMENT_BLOCK_INDEX, fp#FPMul, @q_d_1, @q_1, @azm_temp_1)
	'azm_temp_2 = azm_temp_0 - azm_temp_1
	fp.AddInstruction(MOMENT_BLOCK_INDEX, fp#FPSub, @azm_temp_0, @azm_temp_1, @azm_temp_2)
	'azm_temp_3 = q_d_2 * q_2
	fp.AddInstruction(MOMENT_BLOCK_INDEX, fp#FPMul, @q_d_2, @q_2, @azm_temp_3)
	'azm_temp_4 = q_d_3 * q_3
	fp.AddInstruction(MOMENT_BLOCK_INDEX, fp#FPMul, @q_d_3, @q_3, @azm_temp_4)
	'azm_temp_5 = azm_temp_3 - azm_temp_4
	fp.AddInstruction(MOMENT_BLOCK_INDEX, fp#FPSub, @azm_temp_3, @azm_temp_4, @azm_temp_5)
	'q_tilde_0 = azm_temp_2 - azm_temp_5
	fp.AddInstruction(MOMENT_BLOCK_INDEX, fp#FPSub, @azm_temp_2, @azm_temp_5, @q_tilde_0)

This could then be run in a special version of F32 that would chug though the math instruction at full assembly speed. In my testing I found that much of the slowness of F32 was the Spin overhead, and so if you can put your math operations back to back things get alot faster.

https://github.com/srlm-io/anzhelka/blob/master/software/spin/lib/F32_CMD.spin

I don't really suggest that you use the code as is, but maybe it will provide some inspiration. Also in that repo is a pretty handy doc walking through the implementation of a quadrotor control system.

https://github.com/srlm-io/anzhelka/raw/master/doc/reports/quadrotor_mathematics.pdf

Reinhard

http://rosettacode.org/wiki/Simple_Quaternion_type_and_operations#C

maybe this is helpfull
Reinhard