Ok, so I have my Arduino hooked up to an MPU-6050 6-axis gyro+accelerometer GY-521 breakout board. I've got it sending serial data to a program on my computer, all working perfectly. I'll pose this question in C++ even though my program is written in DBPro.
I'm currently using a complementary filter to combine the gyro rates of angular rotation with the accelerometer gravity vector for stable yet responsive rotations. Here's my current code:
angleX=0.98*(angleX+(gyroX/16.4)*timeMultiplier)+0.02*atanfull(accelY,accelZ)
angleY=0.98*(angleY+(gyroY/16.4)*timeMultiplier)+0.02*atanfull(accelZ,accelX)
angleZ=0.98*(angleZ+(gyroZ/16.4)*timeMultiplier)+0.02*atanfull(accelX,accelZ)
angleX/Y/Z and timeMultiplier are floats. timeMultiplier is basically (millis()-lastMillis)/1000.0. atanfull is basically atan2 but outputs in degrees (0-360), not radians (I think that's the only difference).
The issue is that when the board is sitting flat on the table, the accelerometer obviously can't measure rotations about the Y axis (up), but the gyro can. When I twist the board around Y, the 3D cube representation I'm rendering twists for a second then is pulled back to 0. That's because when the board is upright (with the Y axis pointing upwards) the atanfull results in 0.
What I've been scouring the internet for (but really don't know what to search) is a way to reduce the affect of the accelerometer data the less use it becomes. So as the X axis, say, is rotated so it's perpendicular to the ground (and becomes useless for measurements) the gyro is allowed more and more control, until when the X axis becomes fully perpendicular the gyro is taking no notice of the accelerometer for that axis and relies fully on the gyro.
TL;DR: I'm trying to find a way to combine gyro and accelerometer readings in a rotation-independent way. That is, all the implementations of gyro+accel complementary filters I can find handle pitch/roll differently to yaw, and I don't want to do that. I want to be able to turn my board in any direction and still get good readings.
I really have no idea what I should be searching for to find out how to do this, and would appreciate any help :)
Related
I've spent a few hours doing research on numeric integration and velocity/position estimation but I couldn't really find an answer that would be either understandable by my brain or appropriate for my situation.
I have an IMU (Inertial Measurement Unit) that has a gyro, an accelerometer and a magnetometer. All those sensors are in fusion, which means for example using the gyro I'm able to compensate for the gravity in the accelerometer readings and the magnetometer compensates the drift.
In other words, I can get pure acceleration readings using such setup.
Now, I'm trying to accurately estimate the position based on acceleration, which as you may know requires double integration, and there are various methods of doing that. But I don't know which would be the most appropriate here.
Could somebody please share some information about this?
Also, I'd appreciate it if you could explain it to me without using any complex math formulas/symbols, I'm not a mathematician and this was one of my problems when looking for information.
Thanks
You can integrate the accelerations by simply summing the acceleration vectors multiplied by the timestep (period of the IMU) to get the velocity, then sum the velocities times the timestep to get the position. You can propagate (not integrate) the orientation by using various methods depending on which orientation representation you choose (Euler angles, Quaternions, Attitude Matrix (DCM), Axis-Angle, etc..).
However you have a bigger problem.
Long story short: Unless you have a naval-quality IMU (USD$200,000+) you cannot simply integrate the accelerations and angular rates to get an accurate pose (position and orientation) estimates.
I assume you are using a low-cost (under USD$1,000) IMU - your accelerometer and gyro are subject to both noise and bias. These will make it impossible to get accurate pose by simply integrating.
In practice to do what you are intending it is required to fuse 'correcting' measurements of the position and optionally the orientation. The IMU 'predicts' the position/orientation, while another sensor model (camera features, gps, altimeter, range/bearing measurements) take the predicted position and 'correct' it. There are various methods of fusing this data, the most prolific of which is the Extended Kalman Filter or Error-State (Indirect) Kalman Filter.
Back to your original question; I would represent the orientation as quaternions, and you can propagate the quaternion orientation by using the error-quaternion derivative and the angular rates from your gyro.
EDIT:
The noise problem can be partially worked around by using a high pass
filter, but what kind of bias are you exactly talking about?
You should read up on the sources of error in MEMS accelerometers: constant alignment bias, random walk bias, white noise and temperature bias. As you said you can high-pass filter to reduce the effect of noise - however this is not perfect so there is significant residual noise. The double integration of the residual noise gives a quadratically-increasing position error. Even after removing the acceleration due to gravity there will be significant accelerations measured due to these error sources which will render the position estimate inaccurate within less than 1 second of integration.
I'm accessing the Kinect Accelerometer in c++ via openFrameworks and ofxKinect and am having some issues with certain angles. If I pitch the kinect 90 degrees downwards I get nan values. I had a look at the getAccelPitch() method and this kind of makes sense since asin will return 0 when there will be values greater than 9.80665 divided by 10.1/9.80665.
The main problem though is after I pitch the device 90 degrees, the roll doesn't seem reliable(doesn't seem change much). In my setup I will need to have the device pitched 90 degrees but also know it's new roll.
Any hints,tips on how I may do that ? Is there an easy way to get the data to draw the kinect's orientation with 3 lines(axes).
I'm trying to detect orientations like these:
The problem is that you are using Euler angles (roll, pitch and yaw).
Euler angles are evil and they screw up the stability of your app, see for example
Strange behavior with android orientation sensor,
Reducing wiimote pitch/roll variations.
They are not useful for interpolation either.
A solution is to use rotation matrices instead. A tutorial on rotation matrices is given in the
Direction Cosine Matrix IMU: Theory
manuscript.
I d like to make my Camera shake in opengl
my old algorithm is:
setCamPosXYZ(x+randx,y+randy,z+randz);
but it is not the best. What is a better solution, a real world camera shake?
I have found a sin function with a high frequency and a gradually decreasing amplitude looks very convincing. If you want it to appear more random you can have different frequencies for each direction and it will bounce around more randomly. A sin function is also makes sense physically because if you think of the camera being mounted on a very stiff spring with some damping, then it's going to oscillate like a damped sin curve.
Generate x+randx,y+randy,z+randz
and then animate smoothly (liner interpolation) between existing camera pos
and x+randx,y+randy,z+randz
for a smooth shake.
To animate smoothly set your new
x at each time step to xinitial + k(randx)
Once you reach the random shake point generate a new random point and again animate
using linear interpolation
A real world camera shake is a reaction to some force, like a bumb or a shockwave, so it's a movement in one direct, then a countermovement in the other direction, moving back and forth until it stabilises.
So start with a sinus wave that dissipates, and then you might add some random to this to make it a bit more jerky.
Also, you should add a slight rotation of the camera along with the movement.
Ok i know this is quite off-topic for programmers but still I need this for app, so here it is:
Ballistic curve (without wind or any other conditions) is specified by these 2 lines:
So, there is a problem that you got 3 unknown values: x,y and time t, but only 2 equations.
You can't really compute all 3 with just these values, I got:
velocity v
angle Alpha
origin coordinates
Thus, you have to decide which one to specify.
Now you have 2D tanks game, or anything like that, you know you have tank and using ballistic you have to shoot opponent down with setting angle and power.
I need to know when the bullet hit the ground, it can be on-air as it fly, or precomputed.
There comes up my problem. Which way to use? Pre-compute or check for hitting the ground in each step.
If I would like to pre-compute, I would need to know height of terrain, which, logically would have to be constant as I don't know in which x coord. If I would know the X, it would mean that just in front of my turret is wall. So only way to get to result, when I hit the ground, would be with checking in intervals of time for hitting the ground. This is also good because the terrain doesn't have top be static yay! But isn't that too great overhead which could be made much simpler? Have you encountered with such problem/solution?
Thanks in advance, btw the terrain can be flat, using lines or NURBS so I please for general solution, not specific as in which height you shoot in that will be impact.
You can compute the path of the projectile y(x) by solving one equation for t and substituting into the other. You get
Then finding the landing point is a matter of computing the intersections between that function and the function that defines the height of the terrain. One intersection will be the launch point and the other will be the landing point. (If your terrain is very steep and hilly, there could be more than 2 intersections, in which case you take the first one with x greater than the launch point.) You can use any of various root-finding algorithms to actually compute the intersection; check the documentation of whatever mathematical or game-physical libraries you have to see if they provide a method to do this.
David Zaslavsky did a good job of answering your question about solving for the equation, but if your ultimate goal is simple ballistics simluation, I suggest you instead use vector decomposition.
By utilizing vector decomposition, you can derive the x- and y-compenent vectors of your projectile. You can then apply acceleration to each component to account for gravity, wind, etc. Then you can update the projectile's (x,y) position each interval as a function of time.
For example:
double Speed = 100.0; // Speed rather than velocity, as it is only the magnitude
double Angle = 30.0; // Initial angle of 30º
doulbe Position[2] = {0.0,0.0}; // Set the origin to (0,0)
double xvelocity = Speed * Cos(Angle);
double yvelocity = Speed * Sin(Angle);
Then if you can impliment a simple Update function as follows:
void Update(double Time)
{
yvelocity = -9.8 * Time; // Apply gravity
Position[0] *= (xvelocity * Time); // update x position
Position[1] *= (yvelocity * time); // update y position
CheckCollisions(); // check for collisions
}
Of course this is a very basic example, but you can build on it from here.
Fortunately, this is pretty simple kinematics.
Those equations are parametric: For any given time t, they give you the x and y coordinates for that time. All you need to do is plug in the starting velocity v and angle a.
If you're working on level ground, the time for your projectile to come back down is simply 2sin(a)v/g, i.e. the vertical component of your velocity divided by the downward acceleration due to gravity. The 2 is because it takes that amount of time for the speed to drop to 0, then the same time again for it to accelerate back down. Once you know the time you can solve for x.
If your terrain is not flat, you have some additional fun. Something you could try is work out the time for hitting the ground at the same height, and then correct for the extra vertical distance. This will also change your horizontal distance which might again affect your height... but two or three adjustments and the error will be too small for humans to notice :)
I'm not sure you're going about this is right way. The main equation you want is s = si + vi*dt + .5*adtdt. This is a simple equation of one dimension, but it generalizes to vectors cleanly.
Effectively, si is your initial position and vi is your initial velocity and a is acceleration due to gravity.
To make this work, build a vector for perfect horizontal muzzle velocity and project it on the launch angle. That's your vi. Si will be the tip of the barrel. From there it's vector summing and scaling.
Continuous functions do not work well for computers, because computers are implicitly discrete: the floating/double numbers are discrete, the timer is discrete, the game grid is discrete (even if it uses 'doubles').
So just discretize the equation the way Justin Holdsclaw suggested. Have velocity and accelerations vectors in each direction (in your case X and Y; you could also add Z). Update all vectors, and the object's position in space, at every tick.
Note that the result won't be 'exact'. The smaller your 'delta' values (grid coarseness), the closer you'll be to the 'exact' curve. To know precisely how close - if you're interested, find a book on numerical analysis and read the first few chapters. For practical purposes you can just experiment a bit.
I have a video file recorded from the front of a moving vehicle. I am going to use OpenCV for object detection and recognition but I'm stuck on one aspect. How can I determine the distance from a recognized object.
I can know my current speed and real-world GPS position but that is all. I can't make any assumptions about the object I'm tracking. I am planning to use this to track and follow objects without colliding with them. Ideally I would like to use this data to derive the object's real-world position, which I could do if I could determine the distance from the camera to the object.
Your problem's quite standard in the field.
Firstly,
you need to calibrate your camera. This can be done offline (makes life much simpler) or online through self-calibration.
Calibrate it offline - please.
Secondly,
Once you have the calibration matrix of the camera K, determine the projection matrix of the camera in a successive scene (you need to use parallax as mentioned by others). This is described well in this OpenCV tutorial.
You'll have to use the GPS information to find the relative orientation between the cameras in the successive scenes (that might be problematic due to noise inherent in most GPS units), i.e. the R and t mentioned in the tutorial or the rotation and translation between the two cameras.
Once you've resolved all that, you'll have two projection matrices --- representations of the cameras at those successive scenes. Using one of these so-called camera matrices, you can "project" a 3D point M on the scene to the 2D image of the camera on to pixel coordinate m (as in the tutorial).
We will use this to triangulate the real 3D point from 2D points found in your video.
Thirdly,
use an interest point detector to track the same point in your video which lies on the object of interest. There are several detectors available, I recommend SURF since you have OpenCV which also has several other detectors like Shi-Tomasi corners, Harris, etc.
Fourthly,
Once you've tracked points of your object across the sequence and obtained the corresponding 2D pixel coordinates you must triangulate for the best fitting 3D point given your projection matrix and 2D points.
The above image nicely captures the uncertainty and how a best fitting 3D point is computed. Of course in your case, the cameras are probably in front of each other!
Finally,
Once you've obtained the 3D points on the object, you can easily compute the Euclidean distance between the camera center (which is the origin in most cases) and the point.
Note
This is obviously not easy stuff but it's not that hard either. I recommend Hartley and Zisserman's excellent book Multiple View Geometry which has described everything above in explicit detail with MATLAB code to boot.
Have fun and keep asking questions!
When you have moving video, you can use temporal parallax to determine the relative distance of objects. Parallax: (definition).
The effect would be the same we get with our eyes which which can gain depth perception by looking at the same object from slightly different angles. Since you are moving, you can use two successive video frames to get your slightly different angle.
Using parallax calculations, you can determine the relative size and distance of objects (relative to one another). But, if you want the absolute size and distance, you will need a known point of reference.
You will also need to know the speed and direction being traveled (as well as the video frame rate) in order to do the calculations. You might be able to derive the speed of the vehicle using the visual data but that adds another dimension of complexity.
The technology already exists. Satellites determine topographic prominence (height) by comparing multiple images taken over a short period of time. We use parallax to determine the distance of stars by taking photos of night sky at different points in earth's orbit around the sun. I was able to create 3-D images out of an airplane window by taking two photographs within short succession.
The exact technology and calculations (even if I knew them off the top of my head) are way outside the scope of discussing here. If I can find a decent reference, I will post it here.
You need to identify the same points in the same object on two different frames taken a known distance apart. Since you know the location of the camera in each frame, you have a baseline ( the vector between the two camera positions. Construct a triangle from the known baseline and the angles to the identified points. Trigonometry gives you the length of the unknown sides of the traingles for the known length of the baseline and the known angles between the baseline and the unknown sides.
You can use two cameras, or one camera taking successive shots. So, if your vehicle is moving a 1 m/s and you take fames every second, then successibe frames will gibe you a 1m baseline which should be good to measure the distance of objects up to, say, 5m away. If you need to range objects further away than the frames used need to be further apart - however more distant objects will in view for longer.
Observer at F1 sees target at T with angle a1 to velocity vector. Observer moves distance b to F2. Sees target at T with angle a2.
Required to find r1, range from target at F1
The trigonometric identity for cosine gives
Cos( 90 – a1 ) = x / r1 = c1
Cos( 90 - a2 ) = x / r2 = c2
Cos( a1 ) = (b + z) / r1 = c3
Cos( a2 ) = z / r2 = c4
x is distance to target orthogonal to observer’s velocity vector
z is distance from F2 to intersection with x
Solving for r1
r1 = b / ( c3 – c1 . c4 / c2 )
Two cameras so you can detect parallax. It's what humans do.
edit
Please see ravenspoint's answer for more detail. Also, keep in mind that a single camera with a splitter would probably suffice.
use stereo disparity maps. lots of implementations are afloat, here are some links:
http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/OWENS/LECT11/node4.html
http://www.ece.ucsb.edu/~manj/ece181bS04/L14(morestereo).pdf
In you case you don't have stereo camera, but depth can be evaluated using video
http://www.springerlink.com/content/g0n11713444148l2/
I think the above will be what might help you the most.
research has progressed so far that depth can be evaluated ( though not to a satisfactory extend) from a single monocular image
http://www.cs.cornell.edu/~asaxena/learningdepth/
Someone please correct me if I'm wrong, but it seems to me that if you're going to simply use a single camera and simply relying on a software solution, any processing you might do would be prone to false positives. I highly doubt that there is any processing that could tell the difference between objects that really are at the perceived distance and those which only appear to be at that distance (like the "forced perspective") in movies.
Any chance you could add an ultrasonic sensor?
first, you should calibrate your camera so you can get the relation between the objects positions in the camera plan and their positions in the real world plan, if you are using a single camera you can use the "optical flow technic"
if you are using two cameras you can use the triangulation method to find the real position (it will be easy to find the distance of the objects) but the probem with the second method is the matching, which means how can you find the position of an object 'x' in camera 2 if you already know its position in camera 1, and here you can use the 'SIFT' algorithme.
i just gave you some keywords wish it could help you.
Put and object of known size in the cameras field of view. That way you can have a more objective metric to measure angular distances. Without a second viewpoint/camera you'll be limited to estimating size/distance but at least it won't be a complete guess.