I am well aware of the existence of this question but mine will differ. I also know that there could be significant errors with this approach but I want to understand the configuration also theoretically.
I have some basic questions which I find hard to answer for myself clearly. There is a lot of information about accelerometers and gyroscopes but I still haven't found an explanation "from first principles" of some basic properties.
So I have a plate sensor that contains an accelerometer and gyroscope. There is also a magnetometer which I skip for now.
The accelerometer gives information in each time t about the temporary acceleration vector a = (ax, ay, az) in m/s^2 according to the fixed coordinate system to the sensor.
The gyroscope gives a 3D vector in deg/s which says the temporary speed of rotation of the three axes (Ox, Oy and Oz). From this information, one can get a rotation matrix that corresponds to an infinitesimal rotation of the coordinate system (according to the previous moment). Here is some explanation how to obtain a quaternion, that represents R.
So we know that the infinitesimal movement can be calculated considering that the acceleration is the second derivative of the position.
Imagine that your sensor is attached to your hand or leg. In the first moment we can consider its point in 3D space as (0,0,0) and the initial coordinate system also attached in this physical point. So for the very first time step we will have
r(1) = 0.5a(0)dt^2
where r is the infinitesimal movement vector, a(0) is the acceleration vector.
In each of the following steps we will use the calculations
r(t+1) = 0.5a(t)dt^2 + v(t)dt + r(t)
where v(t) is the speed vector which will be estimated in some way, for example as (r(t)-r(t-1)) / dt.
Also, after each infinitesimal movement we will have to take into account the data from the gyroscope. We will use the rotation matrix to rotate the vector r(t+1).
In this way, probably with tremendous error I will get some trajectory according to the initial coordinate system.
My queries are:
Am I principally correct with this algorithm? If not, where am I wrong?
I would very much appreciate some resources with a working example where the first principles are not skipped.
How should I proceed with using the Kalman's filter to obtain a better trajectory? In what way exactly do I pass all the IMU data (accelerometer, gyroscope and magnetometer) to the Kalman filter?
Your conceptual framework is correct, but the equations need some work. The acceleration is measured in the platform frame, which can rotate very quickly, so it is not advisable to integrate acceleration in the platform frame and rotate the position change. Rather, the accelerations are transformed into a relatively slowly rotating frame and the integration to velocity change and position change is done there. Typically a locally-level frame (e.g. North-East-Down or Wander Aziumuth) or an Earth-centered frame (ECEF or ECI). Gravity and Coriolis force must be included in the acceleration.
Derivations from first principles can be found in many references, one of my favorites is Strapdown Inertial Navigation Technology by Titterton and Weston. Derivations of the inertial navigation equations in locally-level and Earth-fixed frames are given in Chapter 3.
As you've recognized in your question - the initial velocity is an unknown constant of integration. Without some estimate of initial velocity the trajectory resulting from integrating the inertial data can be wildly wrong.
Related
When showing the extrinsic parameters of calibration (the 3D model including the camera position and the position of the calibration checkerboards), the toolbox does not include units for the axes. It seemed logical to assume that they are in mm, but the z values displayed can not possibly be correct if they are indeed in mm. I'm assuming that there is some transformation going on, perhaps having to do with optical coordinates and units, but I can't figure it out from the documentation. Has anyone solved this problem?
If you marked the side length of your squares in mm, then the z-distance shown would be in mm.
I know next to nothing about matlabs (not entirely true but i avoid matlab wherever I can, and that would be almost always possible) tracking utilities but here's some general info.
Pixel dimension on the sensor has nothing to do with the size of the pixel on screen, or in model space. For all purposes a camera produces a picture that has no meaningful units. A tracking process is unaware of the scale of the scene. (the perspective projection takes care of that). You can re insert a scale by taking 2 tracked points and measuring the distance between those points. This is the solver spaces distance is pretty much arbitrary. Now if you know the real distance between these points you can get a conversion factor. By doing:
real distance / solver space distance.
There's really now way to knowing this distance form the cameras settings as the camera is unable to differentiate between different scales of scenes. So a perfect 1:100 replica is no different for the solver than the real deal. So you must allays relate to something you can measure separately for each measuring session. The camera always produces something that's relative in nature.
I am writing an iPhone/iPad app. I need to compute the acceleration and deceleration in the direction of travel of a vehicle traveling in close to a straight horizontal line with erratic acceleration and deceleration. I have the sequence of 3 readings from the X,Y,Z orthogonal accelerometers. But the orientation of the iphone/ipad is arbitrary and the accelerometer readings include vehicle motion and the effect of gravity. The result should be a sequence of single acceleration values which are positive or negative depending on whether the vehicle is decelerating or accelerating. The positive and negative direction is arbitrary so long as acceleration has the opposite sign to deceleration. Gravity should be factored out of the result. Some amount of variable smoothing of the result would be useful.
The solution should be as simple as possible and must be computationally efficient. The answer should be some kind of pseudo-code algorithm, C code or a sequence of equations which could easily be converted to C code. An iPhone specific solution in Objective C would be fine too.
Thanks
You will need some trigonometry for this, for example to get the magnitude you need
magn = sqrt(x*x+y*y+z*z);
to get the angle you will need atan, then c function atan2 is better
xyangel = atan2(y,x);
xymagn = sqrt(x*x+y*y);
vertangle = atan2(z,xymagn)
no how you get negative and positive magnitude is arbitrary, you could for example interpret π/2 < xyangle < 3π/2 as negative. That would be taking the sign of x for the sign of magn, but it would be equally valid to take the sign from y
It is really tough to separate gravity and motion. It's easier if you can analyze the data together with a gyroscope and compass signal.
The gyroscope measures the rate of angular rotation. Its integral is theoretically the angular orientation (plus an unknown constant), but the integral is subject to drift, so is useless on its own. The accelerometer measures angular orientation plus gravity plus linear acceleration. With some moderately complex math, you can isolate all 3 of those quantities from the 2 sensors' values. Adding the compass fixes the XY plane (where Z is gravity) to an absolute coordinate frame.
See this great presentation.
Use userAcceleration.
You don't have to figure out how to remove gravity from the accelerometer readings and how to take into accont the orientation: It is already implemeted in the Core Motion Framework.
Track the mean value of acceleration. That will give you a reference for "down". Then subtract the mean from individual readings.
You'll need to play around with the sensitivity of the mean calculation, since, e.g., making a long slow turn on a freeway will cause the mean to slowly drift outwards.
If you wanted to compensate for this, you could use GPS tracking to compute a coarse-grained global acceleration to calibrate the accelerometer. In fact, you might find that differentiating the GPS velocity reading gives a good enough absolute acceleration all by itself (I haven't tried, so I can't say).
I want to use the iPhones's accelerometer to detect motions while driving. I'm a bit confused what the accelerometer actually measures, especially when driving a curve.
As you can see in the picture, a car driving a curve causes two forces. One is the centripetal force and one is the velocity. Imagine the iPhone is placed on the dashboard with +y-axis is pointing to the front, +x-axis to the right and +z-axis to the top.
My Question is now what acceleration will be measured when the car drives this curve. Will it measure g-force on the -x-axis or will the g-force appear on the +y axis?
Thanks for helping!
UPDATE!
For thoses interested, as one of the answers suggested it measures both. The accelerometer is effected by centrifugal force and velocity resulting in an acceleration vector that is a combination of these two.
I think it will measure both. But don't forget that the sensor will measure gravity as well. So when your car is not moving, you will still get accelerometer readings. A nice talk on sensors in smartphones http://www.youtube.com/watch?v=C7JQ7Rpwn2k&feature=results_main&playnext=1&list=PL29AD66D8C4372129 (it's on android, but the same type of sensors are used in iphone).
Accelerometer measures acceleration of resultant force applied to it (velocity is not a force by the way). In this case force is F = g + w + c i.e. vector sum of gravity, centrifugal force (reaction to steering centripetal force, points from the center of the turn) and car acceleration force (a force changing absolute value of instantaneous velocity, points along the velocity vector). Providing Z axis of accelerometer always points along the gravity vector (which is rare case for actual car) values of g, w and c accelerations can be accessed in Z, X and Y coordinates respectively.
Unless you are in free fall the g-force (gravity) is always measured. If I understand your setup correctly, the g-force will appear on the z axis, the axis that is vertical in the Earth frame of reference. I cannot tell whether it will be +z or -z, it is partly convention so you will have to check it for yourself.
UPDATE: If the car is also going up/downhill then you have to take the rotation into account. In other words, there are two frames of reference: the iPhone's frame of reference and the Earth frame of reference. If you would like to deal with this situation, then please ask a new question.
I have an accelerometer and magnetometer each producing raw X, Y and Z readouts. From this I need to determine the magnetic heading of an object.
I'm not that great at trig, but I've put together a formula that does respond pretty well to the rotation of the device, but also responds to movement that one would not think is relevant, such as angling the device in such a way that has no impact on the direction it is pointed. Such as laying it flat and "rolling" the device.
I think the formula I have for calculating the magnetic heading is fine, but I think my pitch and roll radians for input are wrong.
So I guess the core of my question (unless someone actually has a formula around that does this), is how do you calculate angles, in radians, using an accelerometer for pitch and roll.
Then secondly, any info on the heading formula itself would be great.
Depending on the accuracy your application requires, you may need to solve several problems:
Are the accelerometer axes calibrated? I've seen MEMs accelerometers that had axes that were not mutually perpendicular, and had significantly different response characteristics for each axis (typically X and Y would match, and Z would differ). You will need to synthesize ideal XYZ axes from whatever physical reading your device provides. (Google 'accelerometer calibration'.)
Are the magnetometer axes calibrated? Similar problem as above, except much harder to check: It is very difficult to generate uniform calibrated magnetic fields. If you use the ambient geomagnetic field, you will need to carefully control the local magnetism of your work environment and your tools. (Google 'magnetometer calibration'.)
After the accelerometer and magnetometer have been individually calibrated, their axes need to be calibrated relative to each other. Since both of these devices are typically soldered to a PCB, there is almost guaranteed to be significant misalignment. In many cases, the board layout and device parameters may not even permit the XYZ axes to correspond with each other! This may be the hardest part to do from a lab perspective, so I'd recommend you do a direct comparison using other hardware that has both kinds of sensors and is already calibrated (such as an iPhone or Android phone - but verify the device before use). Normally, this is accomplished by adjusting the prior two calibration matrices until they generate vectors that are correctly aligned relative to each other.
Only after you are generating mutually calibrated magnetic and accelerometer vectors can you apply the solutions suggested by the other respondents.
I've only described the static solution, where both the magnetometer and accelerometer are motionless relative to the local gravitational and magnetic fields. If you need to generate responses in real-time while the system is rapidly moving, you will need to account for the time behavior of each sensor. There are two basic ways to do this: 1) Apply time-domain filters to each sensor so that their outputs share a common time domain (generally adding some delay). 2) Use predictive modeling to modify the sensor outputs in real-time (less delay, but more noise).
I've seen Kalman filters used for such applications, but applying them in a vector domain may require using quaternions instead of Euler matrices. Quaternions are easier to use computationally (fewer operations needed compared to matrices), but I found them to be much more difficult to comprehend and get right.
Or, you may choose a completely different path, and use statistics and data fitting to do all the above work in one giant step. Consider the problem as follows: Given 6 input values (XYZ each from uncalibrated magnetometer and accelerometer) and a reference to the device (assuming it is hand-held, and there is an arrow painted on the case), output a single angle representing the magnetic bearing toward which the arrow on the case is pointing, and the elevation of the arrow relative to the gravity vector (tilt of the case).
Using a calibrated reference device (as mentioned above), pair it with the device to be calibrated, and take a several hundred data points, with the device at different orientations. Then use a powerful math package such a Matlab, MathCAD, R or SciPy to setup and solve the nonlinear equations to create the transformation matrices.
I would point to Euler Angles and Roll Pich Yaw.
You're not thinking in enough dimensions. This would be the answer in only 2 dimensions, and it works great if you can find a way to ensure "Z" always aligns with gravity.
int heading=180-atan2(mag_datX, mag_datY)/0.0174532925; // 0/359=N, 90=E, 180=S, 270=W
(if you're reading directly form the device - beware that it probably returns X, Z, Y - not X, Y, Z !)
However - this is not a 2D compass problem - imagine you take the needle out of the compass, balance it so that gravity plays no part in keeping it "level", and you'll find that "north" will point a bit up or down - depending where on earth you are (or, if at the poles, directly up or down!).
So you need to try and compute the THREE DIMENSIONAL vector from all 3 values - which is a matrix operation.
Let's consider a disk with mass m and radius R on a surface where friction u also involved. When we give this disk a starting velocity v in a direction, the disk will go towards that direction and slow down and stop.
In case the disk has a rotation (or spin with the rotational line perpendicular on the surface) w beside the speed then the disk won't move on a line, instead bend. Both the linear and angular velocity would be 0 at the end.
How can this banding/curving/dragging be calculated? Is it possible to give analytical solution for the X(v,w,t) function, where X would give the position of the disk according to it's initial v w at a given t?
Any simulation hint would be also fine.
I imagine, depending on w and m and u there would be an additional velocity which is perpendicular to the linear velocity and so the disk's path would bend from the linear path.
If you're going to simulate this, I'd probably recommend something like dividing up the contact surface between the disk and the table into a radial grid. Compute the relative velocity and the force at each point on the grid at each time step, then sum up the forces and torques (r cross F) to get the net force F and the net torque T on the disk as a whole. Then you can apply the equations F=(m)(dv/dt) and T=(I)(dw/dt) to determine the differential changes in v and w for the next time step.
For what it's worth, I don't think a flat disk would curve under the influence of either a frictional force (velocity-independent) or a drag force (linearly proportional to velocity).
A ball will move in a large arc with spin, but a [uniform] disk on a 2D surface will not.
For the disk it's center of spin is the same as it's center of gravity, so there is no torque applied. (As mentioned by duffymo, a nonuniform disk will have a torque applied.)
For a uniform ball, if the axis of the spin is not perpendicular to the table, this causes the ball to experience a rotational torque which causes it to move in a slight arc. The arc has a large radius, and the torque is slight, so usually friction makes the ball stop quickly.
If there was a sideways velocity, the ball would move along a parabola, like a falling object. The torque component (and the radius of the arc) can be computed in the same way you do for a precessing top. It's just that the ball sits at the tip of the top (err....) and the bottom is "imaginary".
Top equation: http://hyperphysics.phy-astr.gsu.edu/HBASE/top.html
omega_p = mgr/I/omega
where
omega_p = rotational velocity...dependent on how quickly you want friction to slow the ball
m = ball mass
g = 9.8 m/s^2 (constant)
r = distance from c.g. (center of ball) to center, depends on angle of spin axis (solve for this)
omega = spin rate of ball
I = rotational inertia of a sphere
My 2 cents.
Numerical integration of Newton's laws of motion would be what I'd recommend. Draw the free body diagram of the disk, give the initial conditions for the system, and numerically integrate the equations for acceleration and velocity forward in time. You have three degrees of freedom: x, y translation in the plane and the rotation perpendicular to the plane. So you'll have six simultaneous ODEs to solve: rates of change of linear and angular velocities, rates of change for two positions, and the rate of change of angular rotation.
Be warned: friction and contact make that boundary condition between the disk and the table non-linear. It's not a trivial problem.
There could be some simplifications by treating the disk as a point mass. I'd recommend looking at Kane's Dynamics for a good understanding of the physics and how to best formulate the problem.
I'm wondering if the bending of the path that you're imagining would occur with a perfectly balanced disk. I haven't worked it out, so I'm not certain. But if you took a perfectly balanced disk and spun it about its center there'd be no translation without an imbalance, because there's no net force to cause it to translate. Adding in an initial velocity in a given direction wouldn't change that.
But it's easy to see a force that would cause the disk to deviate from the straight path if there was an imbalance in the disk. If I'm correct, you'll have to add an imbalance to your disk to see bending from a straight line. Perhaps someone who's a better physicist than me could weigh in.
When you say friction u, I'm not sure what you mean. Usually there is a coefficient of friction C, such that the friction F of a sliding object = C * contact force.
The disk is modeled as a single object consisting of some number of points arranged in circles about the center.
For simplicity, you might model the disk as a hexagon evenly filled with points, to make sure every point represents equal area.
The weight w of each point is the weight of the portion of the disk that it represents.
It's velocity vector is easily computed from the velocity and rotation rate of the disk.
The drag force at that point is minus its weight times the coefficient of friction, times a unit vector in the direction of its velocity.
If the velocity of a point becomes zero, its drag vector is also zero.
You will probably need to use a tolerance about zero, else it might keep jiggling.
To get the total deceleration force on the disk, sum those drag vectors.
To get the angular deceleration moment, convert each drag vector to an angular moment about the center of the disk, and sum those.
Factor in the mass of the disk and angular inertia, then that should give linear and angular acclerations.
For integrating the equations of motion, make sure your solver can handle abrupt transitions, like when the disk stops.
A simple Euler solver with really fine stepsize might be good enough.