Calculate gravity with inclometer
How would one convert an inclinometers (Pitch, Yaw and Roll) into the gravitational pull expected on the system in [X,Y,Z] ?
A system at rest in a certain Pitch, Yaw and Roll angle should be pulled to earth at a certain [X*g,Y*g,Z*g] , lets say this is for simulation purposes. I want to make a function whoose input is Pitch, Yaw and Roll and the output is a Vector3(X,Y,Z) of the downard moment.
Meaning a object at rest with it's back downwards would output something like [0,-1,0] from the accelerometers and a [pitch,yaw,roll]->[0,-1,0] , where [0,-1,0] minus [0,-1,0] resulting in [0,0,0] . or if we pull it left at the speed 1g we have a accelerometer showing [1,-1,0] making the new value [1,0,0] .
With the system on its back [pitch,yaw,roll]->[0,-1,0] function is what i'm after
Vector3 OriToXYZ(float pitch, float yaw, float roll){
Vector3 XYZ = Vector.Zero;
//Simulate what the XYZ should be on a object in this orientation
//oriented against gravity
...
return XYZ;
}
Yes I know as the explanation below shows I'm not able to detect if the systems upside down or not based on the roll as roll only gives (-90 to 90) but that's a different problem).
This is how the orientation is laid out. 
For extra information about why, what and how to use this information keep reading.
The plan is to use the incinometer as an alternative to the gyrometer for removing the gravity component to the accelerometer data, by simulating/calculating the expected value of gravity at orientation (Pitch,Yaw,Roll).
As the accelerometer(XYZ) is a combination of two components gravity(XYZ) and movement(XYZ), I'm assuming that gravity(XYZ)-calc_g(XYZ) = 0, allowing me to perform accelerometer(XYZ)- calc_g(XYZ) =movement(XYZ)
to show why i think this is possible. when i graph the values from the phone and move the phone sideways hard in a somewhat pendulum motion the lines that looks like sine/cosine motions are inclinometre and the other lines are XYZ accelerometer:
Acceleration value is multiplied by 90 as it ranges from (-2 to 2) so that it in the drawing ranges from -180 to 180, Pitch yaw and roll ranges as seen in the instructable above. The middle of the image is Y = 0, left side is X=0 (X=time)
Solved Solution by Romasz
VectorX = Cos(Pitch)*Sin(Roll);
VectorY = -Sin(Pitch);
VectorZ = -Cos(Pitch)*Cos(Roll);
Result 
*The graphs are not from the same measurement.
(Edited (completely) after comments)
If you want to calculate a gravity components in the direction of inclination then you will need only Pitch and Roll (in WP convention) - the rotation about Z (Yaw) doesn't have influance on accelerometers. The formula should look like this:
VectorX = Cos(Pitch)*Sin(Roll);
VectorY = -Sin(Pitch);
VectorZ = -Cos(Pitch)*Cos(Roll);
(Similar formula you can find for example here or here)
There can be some problems with accuracy from many reasons:
Also watch out because accelerometers can be overloaded (their range is +-2g) - for example if you snap the phone.
To test it I have writted a simple App (which you can download here) - comparing Values indicated by Accelerometers and those calculated via inclination. Because values of accelerometers are relative to gravity, its strightforward:
In XAML - few TextBlocks:
<Grid x:Name="LayoutRoot" Background="Transparent" Margin="20">
<Grid.RowDefinitions>
<RowDefinition Height="1*"/>
<RowDefinition Height="1*"/>
<RowDefinition Height="1*"/>
</Grid.RowDefinitions>
<StackPanel Grid.Row="0" VerticalAlignment="Center" Orientation="Horizontal">
<TextBlock Text="Incliation:" FontSize="16"/>
<TextBlock Name="incXTB" Margin="10"/>
<TextBlock Name="incYTB" Margin="10"/>
</StackPanel>
<StackPanel Grid.Row="1" VerticalAlignment="Center" Orientation="Horizontal">
<TextBlock Text="Accelerometers:" FontSize="16"/>
<TextBlock Name="accXTB" Margin="10"/>
<TextBlock Name="accYTB" Margin="10"/>
<TextBlock Name="accZTB" Margin="10"/>
</StackPanel>
<StackPanel Grid.Row="2" VerticalAlignment="Center" Orientation="Horizontal">
<TextBlock Text="Through Inc:" FontSize="16"/>
<TextBlock Name="accincXTB" Margin="10"/>
<TextBlock Name="accincYTB" Margin="10"/>
<TextBlock Name="accincZTB" Margin="10"/>
</StackPanel>
</Grid>
In code behind:
public partial class MainPage : PhoneApplicationPage
{
private Inclinometer myIncMeter = null;
private float inclX = 0;
private float inclY = 0;
private Accelerometer myAccel = null;
private double accX = 0;
private double accY = 0;
private double accZ = 0;
public MainPage()
{
InitializeComponent();
this.DataContext = this;
myIncMeter = Inclinometer.GetDefault();
myIncMeter.ReportInterval = myIncMeter.MinimumReportInterval;
myAccel = Accelerometer.GetDefault();
myAccel.ReportInterval = myIncMeter.MinimumReportInterval;
CompositionTarget.Rendering += CompositionTarget_Rendering;
}
private void CompositionTarget_Rendering(object sender, EventArgs e)
{
InclinometerReading incRead = myIncMeter.GetCurrentReading();
AccelerometerReading accRead = myAccel.GetCurrentReading();
accX = accRead.AccelerationX;
accY = accRead.AccelerationY;
accZ = accRead.AccelerationZ;
inclX = incRead.RollDegrees;
inclY = incRead.PitchDegrees;
incXTB.Text = "X: " + inclX.ToString("0.00");
incYTB.Text = "Y: " + inclY.ToString("0.00");
accXTB.Text = "X: " + accX.ToString("0.00");
accYTB.Text = "Y: " + accY.ToString("0.00");
accZTB.Text = "Z: " + accZ.ToString("0.00");
accincXTB.Text = "X: " + ((Math.Cos(inclY * Math.PI / 180) * Math.Sin(inclX * Math.PI / 180))).ToString("0.00");
accincYTB.Text = "Y: " + (-Math.Sin(inclY * Math.PI / 180)).ToString("0.00");
accincZTB.Text = "Z: " + (-(Math.Cos(inclX * Math.PI / 180) * Math.Cos(inclY * Math.PI / 180))).ToString("0.00");
}
}
First things first: accelerometers do not measure gravity. They measure acceleration due to every real force but gravity. (That's the Newtonian explanation. The relativistic explanation is even easier: Accelerometers measure acceleration due to all of the real forces acting on the accelerometer. Gravity is a fictitious force in general relativity.)
The first clue that accelerometers don't sense gravity is to look at the output of an accelerometer resting of the surface of the Earth. It registers an acceleration of about 1g upwards. The forces acting on the accelerometer are gravity, about 1g directed downwards, and the normal force, about 1g directed upwards. If accelerometers did sense gravity an accelerometer at rest on the surface of the Earth would register close to zero acceleration. They don't. All they are sensitive to is that 1g upwards normal force.
Another clue: Strap an accelerometer to a skydiver. While the skydiver is standing in the plane and waiting for the plane to reach the drop spot. The floor of the plane pushes the skydiver upwards, and that force propagates throughout the skydiver's body to the accelerometer. The accelerometer will register about 1g upwards. When the skydiver jumps, the accelerometer will suddenly register a sideways acceleration because the only force acting on the skydiver is the horizontal wind. There will be no upward or downward component to the registered acceleration. The drag force will shift to upwards as the skydiver falls and picks up vertical speed, making the accelerometer's output shift from sideways to upwards. The accelerometer output will spike when the skydiver pulls the ripcord and then drop as the skydiver reaches a steady speed. The accelerometer's output has changed drastically even though the gravitational force has hardly changed a bit.
So how to accomplish what you want? Because accelerometers do not sense gravitation, you need some kind of model for gravitation in your software. Depending on need, this model can range in complexity from
The very simple models used in game controllers. Those game control programmers might not even know that they're building a model of the Earth's local gravity field. The model doesn't have to be all that sophisticated as the controller isn't moving much.
The somewhat more sophisticated models used in systems for vehicles that might drive across or fly around town. Some require the operator to start the system and not move until the software says it's okay. During that startup time, the software is calibrating the local gravity field.
The even more sophisticated models used in longer range vehicle systems. Now the curvature of the Earth means "down" changes direction, and also means that the gravitational acceleration varies in magnitude.
The even more sophisticated models used to detect oil fields and such by variations in the gravitational field.
The very sophisticated models used in military planes and satellites that dock with other satellites.
You didn't say what level of sophistication you need. It's probably not the final category; if it was, you wouldn't be asking. You would have taken classes on the subject. You can probably get by with a system that learns the local gravity field. A simple averaging scheme at startup might suffice, or you might need a Kalman filter. The application is going to dictate the accuracy needed, and that in turn will dictated the required sophistication.
链接地址: http://www.djcxy.com/p/65404.html上一篇: 为什么我的D3力布局爆炸
下一篇: 用测角仪计算重力