I'm trying to implement Madgwick sensor fusion algorithm from here on LSM9DS0 sensor (accelerometer, gyroscope and magnetometer) on STM Cortex M3 microcontroller.
Raw data from all sensors seems to be fine.
My problem is: when I hold sensor with it's z-axis horizontal or downwards (i.e. roll or pitch angle is more than 90 degrees) - quaternion from filter becomes really unstable and randomly flips 180 degrees.
More correctly, q0 and q3 are constantly changing signs, resulting in 180 degree flip of rotation.
I tried using constant values instead of real sensor output and still got this behavior.
When z-axis is more or less vertical, quaternion seems plausible.
I haven't seen anything like this on example videos.
I tried to ignore magnetometer data and use 6DOF version of filter but it was a disaster; quaternion is just flying around and spinning uncontrollibly.
I have to set beta-parameter quite large (~100) because lesser values have given very shaky result with sudden flips. I do find this strange since usually beta is about 0.5-0.05.
Filter update frequency is 1KHz.
Can anyone please help me?
Here is said that there is a mistake in code of madgwick filter!
Gradient decent step should look like this:
s0= -_2q2*(2*(q1q3 - q0q2) - ax) + _2q1*(2*(q0q1 + q2q3) - ay) + -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx) + (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my) + _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s1= _2q3*(2*(q1q3 - q0q2) - ax) + _2q0*(2*(q0q1 + q2q3) - ay) + -4*q1*(2*(0.5 - q1q1 - q2q2) - az) + _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx) + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my) + (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s2= -_2q0*(2*(q1q3 - q0q2) - ax) + _2q3*(2*(q0q1 + q2q3) - ay) + (-4*q2)*(2*(0.5 - q1q1 - q2q2) - az) + (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s3= _2q1*(2*(q1q3 - q0q2) - ax) + _2q2*(2*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
and that code on official site is outdated and soon will be replaced.
Correcting this produced a satisfying result.
My other mistake was not reading function prototype properly. I copied slightly changed version of the code, where accelerometer and gyroscope values were interchanged.
Related
I initially coded my simulation using the standard equations of motions, but as is known, it ended up being quite unstable, even if it technically worked.
If we take x(t) to be the position-function of the equations of motion, I calculated my "next" position in the simulation by re-factoring x(t+delta), as follows:
x(t+delta) = x(t) + (1/2)*a*delta^2 + a*t*delta
t = t + delta
Most importantly, a is calculated as NetForce/Mass. If my system/particle encounters a collision, a normal force influences the net force in such a way that it is 0 on the axis of the collision (ie if it falls on the ground, force of gravity is pulling down, normal force of floor cancels gravity). So far, this has worked splendidly with any kind of collision.
However, I decided to switch to Verlet integration as it is known to be more stable, and for some reason, it completely ignores collision. I use the following formulas:
x(t+delta) = x(t) + v(t) * delta + .5*a(t)*delta^2
v(t+delta) = v(t) + .5 * (a(t) + a(t+delta)) * delta
a(t+delta) = NetForce / Mass
t = t + delta
Where v(0)=0, a(0)=0. As such, in addition to old position, I also store old acceleration and velocity.
However, I get the aforementioned problem: It just doesn't work properly, as it ignores collision. Even though it should be factored in into the acceleration already...
How should I approach this?
Isn't it suppose to be:
if x(t) is a position of collision:
v(t+delta) = v(t) - 2*( dot(normal_at(x(t)), v(t)) / norm(normal_at(x(t))^2 )*normal_at(x(t))
x(t+delta) = x(t)
else:
x(t+delta) = x(t) + v(t) * delta + .5*a(t)*delta^2
a(t+delta) = NetForce( x(t+delta) ) / Mass
v(t+delta) = v(t) + .5 * (a(t) + a(t+delta)) * delta
t = t + delta
I have a bunch of drawing areas (they are actually cairo surfaces, but I don't think it matters too much) in a scrolled window, and I would like to refresh the drawings. However, when I redraw the images, they are not shown till I scroll the window up and down. After that the figures are correct, so I have to conclude that the drawing routine itself is proper. I have also included a
while Gtk.events_pending():
Gtk.main_iteration()
loop to wait for all pending operations, but that does not solve the problem. Could someone point out to me what else is missing?
Thanks,
v923z
OK, so the larger chunks of the code. First, a class defining the a drawing area onto which I am going to paint (note that the body is not indented properly! I don't know how to indent larger pieces of code here):
class Preview:
def __init__(self):
self.frame = Gtk.Frame()
self.frame.set_shadow_type(Gtk.ShadowType.IN)
self.frame.show()
self.da = Gtk.DrawingArea()
self.da.set_size_request(200, 300)
self.da.connect('configure-event', self.configure_event)
self.da.connect('draw', self.on_draw)
self.frame.add(self.da)
self.da.show()
def configure_event(self, da, event):
allocation = da.get_allocation()
self.surface = da.get_window().create_similar_surface(cairo.CONTENT_COLOR,
allocation.width,
allocation.height)
cairo_ctx = cairo.Context(self.surface)
cairo_ctx.set_source_rgb(1, 1, 1)
cairo_ctx.paint()
return True
def on_draw(self, da, cairo_ctx):
cairo_ctx.set_source_surface(self.surface, 0, 0)
cairo_ctx.paint()
return True
pass
Next, the point where I actually create the drawing area. viewport_preview is a viewport created in glade.
self.previews = []
self.widget('viewport_preview').remove(self.vbox_preview)
self.vbox_preview = Gtk.VBox(homogeneous=False, spacing=8)
self.widget('viewport_preview').add(self.vbox_preview)
self.vbox_preview.show()
for page in self.pages:
preview = Preview()
self.vbox_preview.pack_start(preview.frame, False, False, 10)
self.previews.append(preview)
while Gtk.events_pending():
Gtk.main_iteration()
self.draw_preview(None)
return True
Then the function drawing the previews. This is really just a wrapper for the next function, and I needed this only because if I delete one entry in the previews, then I have to handle that case. I believe, the while loop at the end of this function is not necessary, for it will be at the end of the next one anyway.
def draw_preview(self, counter=None):
if counter is not None:
self.vbox_preview.remove(self.previews[counter].frame)
self.previews.pop(counter)
self.pages.pop(counter)
self.vbox_preview.show()
while Gtk.events_pending():
Gtk.main_iteration()
for i in range(len(self.pages)):
self.draw_note(self.previews[i].da, self.previews[i].surface, self.pages[i])
while Gtk.events_pending():
Gtk.main_iteration()
Finally, the drawing function itself:
def draw_note(self, widget, surface, page):
list_pos = '%d/%d'%(self.page + 1, len(self.pages))
self.widget('label_status').set_text(list_pos)
cairo_ctx = cairo.Context(surface)
cairo_ctx.set_source_rgb(page.background[0], page.background[1], page.background[2])
cairo_ctx.paint()
width, height = widget.get_size_request()
xmin, xmax, ymin, ymax = fujitsu.page_size(page)
factor = min(height / (2.0 * self.margin + ymax - ymin), width / (2.0 * self.margin + xmax - xmin))
factor *= 0.8
page.scale = factor
value = self.widget('adjustment_smooth').get_value()
#print value
for pen in page.pagecontent:
x = self.margin + pen.path[0][0] - xmin
y = self.margin + pen.path[0][1] - ymin
cairo_ctx.move_to(x * factor, y * factor)
if self.widget('checkbutton_smooth').get_active() == False:
[cairo_ctx.line_to((self.margin + x - xmin) * factor,
(self.margin + y - ymin) * factor) for x, y in pen.path]
else:
bezier_curve = bezier.expand_coords(pen.path, value)
x = self.margin + bezier_curve[0][0][0] - xmin
y = self.margin + bezier_curve[0][0][1] - ymin
cairo_ctx.move_to(x * factor, y * factor)
[cairo_ctx.curve_to((self.margin + control[1][0] - xmin) * factor,
(self.margin + control[1][1] - ymin) * factor,
(self.margin + control[2][0] - xmin) * factor,
(self.margin + control[2][1] - ymin) * factor,
(self.margin + control[3][0] - xmin) * factor,
(self.margin + control[3][1] - ymin) * factor)
for control in bezier_curve]
cairo_ctx.set_line_width(pen.thickness * self.zoom_factor)
cairo_ctx.set_source_rgba(pen.colour[0], pen.colour[1], pen.colour[2], pen.colour[3])
cairo_ctx.stroke()
cairo_ctx.rectangle(0, height * 0.96, width, height)
cairo_ctx.set_source_rgba(page.banner_text[0][0], page.banner_text[0][1], page.banner_text[0][2], page.banner_text[0][3])
cairo_ctx.fill()
cairo_ctx.move_to(width * 0.05, height * 0.99)
cairo_ctx.show_text(self.filename + ' ' + list_pos)
cairo_ctx.set_font_size(self.zoom_factor * 10.0)
xbearing, ybearing, twidth, theight, xadvance, yadvance = (cairo_ctx.text_extents(page.banner_text[3]))
cairo_ctx.move_to(width - 1.03 * twidth, height * 0.99)
cairo_ctx.show_text(page.banner_text[3])
cairo_ctx.set_source_rgba(0, 0, 0.9, 0.90)
cairo_ctx.stroke()
rect = widget.get_allocation()
widget.get_window().invalidate_rect(rect, False)
while Gtk.events_pending():
Gtk.main_iteration()
I think that's about it.
You could use gtk_widget_queue_draw_area or gdk_window_invalidate_rect.This will mark the widget (or rectangle) as dirty and once the main loop is idle expose event will be received where in you can redraw. From you description it appears the updates are happening on expose event so these APIs might be of use. Also you can check this sample from the cairo site where in you can see the usage of gtk_widget_queue_draw_area.
I have not used pygtk but from Google I found that the corresponding call for gtk_widget_queue_draw_area is gtk.Widget.queue_draw_area & for gdk_window_invalidate_rect is gtk.gdk.Window.invalidate_rect
Hope this helps!
Still on the BigNerdRanch iOS Development book.
In the Accelerometer chapter, they first implement accelerometer tracking but it's fairly jumpy. They then suggest to apply a low pass filter to it by changing the original code:
- (void)accelerometer:(UIAccelerometer *)accelerometer didAccelerate:(UIAcceleration *)acceleration
{
HypnosisView *hv = (HypnosisView *)[self view];
[hv setXShift:10.0 * [acceleration x]];
[hv setYShift:10.0 * [acceleration y]];
[hv setNeedsDisplay];
}
to this:
- (void)accelerometer:(UIAccelerometer *)accelerometer didAccelerate:(UIAcceleration *)acceleration
{
HypnosisView *hv = (HypnosisView *)[self view];
float xShift = [hv xShift] * 0.8 + [accel x] * 2.0;
float yShift = [hv yShift] * 0.8 + [accel y] * 2.0;
[hv setXShift:xShift];
[hv setYShift:yShift];
[hv setNeedsDisplay];
}
Fairly simple question: where do they get these values from? I've been looking through the documentation and I found something about low pass filters, which suggests the following code:
#define kFilteringFactor 0.1
- (void)accelerometer:(UIAccelerometer *)accelerometer didAccelerate:(UIAcceleration *)acceleration {
// Use a basic low-pass filter to keep only the gravity component of each axis.
accelX = (acceleration.x * kFilteringFactor) + (accelX * (1.0 - kFilteringFactor));
accelY = (acceleration.y * kFilteringFactor) + (accelY * (1.0 - kFilteringFactor));
accelZ = (acceleration.z * kFilteringFactor) + (accelZ * (1.0 - kFilteringFactor));
// Use the acceleration data.
}
However, I first tried with that code and I got an error (by analyzing my app) saying 'the left value of '*' is a garbage value'. My accelerometer tracking didn't work either.
I'm fairly confused as to what these values mean. For example, in the first part of the code, why do they multiply the acceleration values by 10? To get a 'bigger' movement? I could make some sense out of that, but the second code with the low pass filter makes absolutely no sense to me.
accelX = (acceleration.x * kFilteringFactor) + (accelX * (1.0 - kFilteringFactor));
Whats happening in this code you are multiplying the acceleration at the moment by the Filtering factor 0.1 and then adding it to the filtered acceleration of the last time an update was called by 0.9.
This is pretty much getting the new value and adding it as 10% of the total accelX the other 90% is made up of the previous value which depends on the value before that, which depends on the value before that and so on. This cuts out high frequency values as only allows 10% of any change to go through to the new accelX value.
The KFilteringFactor of 0.1 makes this filter cut out all high frequencies. You will definitely want to experiment by changing this value to suit your particular application.
Since you're working through the Big Nerd Ranch Book - a good idea would be to go on to the Book's discussion forum.
For more information have a look at the Wikepedia article about low pass filters.
And for another example of filtering have a look at Apple's AccelerometerGraph example
Also - think if you take kFilteringFactor to be 0.2 which gives the multipliers for the current value to be 0.8 which is 1 - 0.2, and the multiplier for the new value is 2.0 because it's 0.2 x 10
I suppose 10 is the scaling factor to give reasonable values.
I just trying know how to implement in objective c an equation that gave my position in lat, long and the position of a point in lat,long returns the relative angle to the azimuth so I will be able to know when to paint this point knowing the azimuth. I also know more or less the equation but I don´t know how to do it in objective c:
b = arccos ( cos (90 - lat2) * cos (90 - lat1) + sin (90 - lat2) * sin (90 - lat1) * cos (lon2 - lon1) )
A = arcsin ( sin (90 - lat2) * sin (lon2 - lon1) / sin (b) )
Regarding turning that equation into Objecive-C.
'man cos' and 'man sin' say they take radians, so you'll have to convert your degree values to radians. The common method seems to be this, (90 * M_PI/180), where 90 is the degree value being converted.
The functions for arccos and arcsin are acos and asin respectively. I assume they to take radians. Their man pages don't explicitly say.
Read the man pages to learn which version of the above functions are appropriate for your case. For instance, sin has; sin(), sinl(), sinf().
has anybody already programmed a iphone compass heading tilt compensation?
i have got some approaches, but some help or a better solution would be cool!
FIRST
i define a vector Ev, calculated out of the cross product of Gv and Hv. Gv is a gravity vector i build out of the accelerometer values and Hv is an heading vector built out the magnetometer values.
Ev stands perpendicular on Gv and Hv, so it is heading to horizonatl East.
SECOND
i define a vector Rv, calculated out of the cross product Bv and Gv. Bv is my looking vector and it is defined as [0,0,-1]. Rv is perpendicular to Gv and Bv and shows always to the right.
THIRD
the angle between these two vectors, Ev and Rv, should be my corrected heading. to calculate the angle i build the dot product and thereof the arcos.
phi = arcos ( Ev * Rv / |Ev| * |Rv| )
Theoretically it should work, but maybe i have to normalize the vectors?!
Has anybody got a solution for this?
Thanks, m01d
Yep. You DEFINITELY have to normalize.
This is from my code that I use to extract the orientation of the device.
Gravity is obtained as the x,y,z of the accelerometer
and compass is obtained from the x,y,z of the heading function
gravity.normalize();
compass.normalize();
compassEast=gravity.cross(compass);
compassEast.normalize();
compassNorth=compassEast.cross(gravity);
compassNorth.normalize();
Let me know if you need the full code.
Also, for those who havnt yet seen the iphone 4s gyroscope in action: its amazing! I swapped the above input to gravity and compass for the equivalents from the gyro and the result is stable and smooth and awesome :) Go Apple.
I didn't receive the source code but I set up my own example. You can see the project and code here: http://www.sundh.com/blog/2011/09/stabalize-compass-of-iphone-with-gyroscope/
yes, i did it like described above. but the result is not very accurate. i think with smoother accelerometer values it should work that way. because of this i have choosen to do the tilt compensation by adding/subtracting the accelermoter values of the corresponding axis to/from the compass values.
Here iss my code for the solution above, but its not a final working solution:
- (void)locationManager:(CLLocationManager *)manager didUpdateHeading:(CLHeading *)newHeading{
if (newHeading != nil) {
float Ax = accelerationValueX;
float Ay = accelerationValueY;
float Az = accelerationValueZ;
float filterFactor = 0.2;
Mx = [newHeading x] * filterFactor + (Mx * (1.0 - filterFactor));
My = [newHeading y] * filterFactor + (My * (1.0 - filterFactor));
Mz = [newHeading z] * filterFactor + (Mz * (1.0 - filterFactor));
float counter = ( -pow(Ax, 2)*Mz + Ax*Az*Mx - pow(Ay, 2)*Mz + Ay*Az*My );
float denominator = ( sqrt( pow((My*Az-Mz*Ay), 2) + pow((Mz*Ax-Mx*Az), 2) + pow((Mx*Ay-My*Ax), 2) ) * sqrt(pow(Ay, 2)+pow(-Ax, 2)) );
headingCorrected = (acos(counter/denominator)* (180.0 / M_PI)) * filterFactor + (headingCorrected * (1.0 - filterFactor));
}
...
}