C# 计算智能手机位置的算法——GPS和传感器
我正在开发一个android应用程序,根据传感器的数据计算位置C# 计算智能手机位置的算法——GPS和传感器,c#,android,algorithm,gps,android-sensors,C#,Android,Algorithm,Gps,Android Sensors,我正在开发一个android应用程序,根据传感器的数据计算位置 加速度计-->计算线性加速度 磁强计+加速计-->运动方向 初始位置将取自GPS(纬度+经度) 现在,根据传感器读数,我需要计算智能手机的新位置: 我的算法如下-(但不是计算准确的位置):请帮助我改进它 注意:我的算法代码是C#(我正在将传感器数据发送到服务器-数据存储在数据库中。我正在计算服务器上的位置) 所有DateTime对象都是使用时间戳计算的-从1970年1月1日起 var prevLocation = Serve
var prevLocation = ServerHandler.getLatestPosition(IMEI);
var newLocation = new ReceivedDataDTO()
{
LocationDataDto = new LocationDataDTO(),
UsersDto = new UsersDTO(),
DeviceDto = new DeviceDTO(),
SensorDataDto = new SensorDataDTO()
};
//First Reading
if (prevLocation.Latitude == null)
{
//Save GPS Readings
newLocation.LocationDataDto.DeviceId = ServerHandler.GetDeviceIdByIMEI(IMEI);
newLocation.LocationDataDto.Latitude = Latitude;
newLocation.LocationDataDto.Longitude = Longitude;
newLocation.LocationDataDto.Acceleration = float.Parse(currentAcceleration);
newLocation.LocationDataDto.Direction = float.Parse(currentDirection);
newLocation.LocationDataDto.Speed = (float) 0.0;
newLocation.LocationDataDto.ReadingDateTime = date;
newLocation.DeviceDto.IMEI = IMEI;
// saving to database
ServerHandler.SaveReceivedData(newLocation);
return;
}
//If Previous Position not NULL --> Calculate New Position
**//Algorithm Starts HERE**
var oldLatitude = Double.Parse(prevLocation.Latitude);
var oldLongitude = Double.Parse(prevLocation.Longitude);
var direction = Double.Parse(currentDirection);
Double initialVelocity = prevLocation.Speed;
//Get Current Time to calculate time Travelling - In seconds
var secondsTravelling = date - tripStartTime;
var t = secondsTravelling.TotalSeconds;
//Calculate Distance using physice formula, s= Vi * t + 0.5 * a * t^2
// distanceTravelled = initialVelocity * timeTravelling + 0.5 * currentAcceleration * timeTravelling * timeTravelling;
var distanceTravelled = initialVelocity * t + 0.5 * Double.Parse(currentAcceleration) * t * t;
//Calculate the Final Velocity/ Speed of the device.
// this Final Velocity is the Initil Velocity of the next reading
//Physics Formula: Vf = Vi + a * t
var finalvelocity = initialVelocity + Double.Parse(currentAcceleration) * t;
//Convert from Degree to Radians (For Formula)
oldLatitude = Math.PI * oldLatitude / 180;
oldLongitude = Math.PI * oldLongitude / 180;
direction = Math.PI * direction / 180.0;
//Calculate the New Longitude and Latitude
var newLatitude = Math.Asin(Math.Sin(oldLatitude) * Math.Cos(distanceTravelled / earthRadius) + Math.Cos(oldLatitude) * Math.Sin(distanceTravelled / earthRadius) * Math.Cos(direction));
var newLongitude = oldLongitude + Math.Atan2(Math.Sin(direction) * Math.Sin(distanceTravelled / earthRadius) * Math.Cos(oldLatitude), Math.Cos(distanceTravelled / earthRadius) - Math.Sin(oldLatitude) * Math.Sin(newLatitude));
//Convert From Radian to degree/Decimal
newLatitude = 180 * newLatitude / Math.PI;
newLongitude = 180 * newLongitude / Math.PI;
这是我得到的结果-->手机没有移动。如您所见,速度为27.326311114502,因此计算速度时有些错误,但我不知道是什么
回答:
// init values
double ax=0.0,ay=0.0,az=0.0; // acceleration [m/s^2]
double vx=0.0,vy=0.0,vz=0.0; // velocity [m/s]
double x=0.0, y=0.0, z=0.0; // position [m]
// iteration inside some timer (dt [seconds] period) ...
ax,ay,az = accelerometer values
vx+=ax*dt; // update speed via integration of acceleration
vy+=ay*dt;
vz+=az*dt;
x+=vx*dt; // update position via integration of velocity
y+=vy*dt;
z+=vz*dt;
// init values
double gx=0.0,gy=-9.81,gz=0.0; // [edit1] background gravity in map coordinate system [m/s^2]
double ax=0.0,ay=0.0,az=0.0; // acceleration [m/s^2]
double vx=0.0,vy=0.0,vz=0.0; // velocity [m/s]
double x=0.0, y=0.0, z=0.0; // position [m]
double dev[9]; // actual device transform matrix ... local coordinate system
(x,y,z) <- GPS position;
// iteration inside some timer (dt [seconds] period) ...
dev <- compass direction
ax,ay,az = accelerometer values (measured in device space)
(ax,ay,az) = dev*(ax,ay,az); // transform acceleration from device space to global map space without any translation to preserve vector magnitude
ax-=gx; // [edit1] remove background gravity (in map coordinate system)
ay-=gy;
az-=gz;
vx+=ax*dt; // update speed (in map coordinate system)
vy+=ay*dt;
vz+=az*dt;
x+=vx*dt; // update position (in map coordinate system)
y+=vy*dt;
z+=vz*dt;
public static void PlotNewPosition(Double newLatitude, Double newLongitude, Float currentDistance,
Float currentAcceleration, Float currentSpeed, Float currentDirection, String dataType) {
LatLng newPosition = new LatLng(newLongitude,newLatitude);
if(dataType == "Sensor"){
tvAcceleration.setText("Speed: " + currentSpeed + " Acceleration: " + currentAcceleration + " Distance: " + currentDistance +" Direction: " + currentDirection + " \n");
map.addMarker(new MarkerOptions()
.position(newPosition)
.title("Position")
.snippet("Sensor Position")
.icon(BitmapDescriptorFactory
.fromResource(R.drawable.line)));
}else if(dataType == "GPSSensor"){
map.addMarker(new MarkerOptions()
.position(newPosition)
.title("PositionCollaborated")
.snippet("GPS Position"));
}
else{
map.addMarker(new MarkerOptions()
.position(newPosition)
.title("Position")
.snippet("New Position")
.icon(BitmapDescriptorFactory
.fromResource(R.drawable.linered)));
}
map.moveCamera(CameraUpdateFactory.newLatLngZoom(newPosition, 18));
}
我找到了一个基于传感器计算位置的解决方案:我在下面发布了一个答案
如果您需要任何帮助,请留下评论
这是与GPS比较的结果(注意:GPS为红色)
你似乎在给自己制造麻烦。您应该能够简单地使用并轻松准确地访问位置、方向、速度等
我会考虑使用它,而不是在服务器端工作。我不太确定,但我最好的猜测是关于这一部分:
Double initialVelocity = prevLocation.Speed;
var t = secondsTravelling.TotalSeconds;
var finalvelocity = initialVelocity + Double.Parse(currentAcceleration) * t;
如果让我们假设在前面的位置,速度是:27.326。。。t==0和currentAcceleration==0(正如您所说的,您处于空闲状态),最终的位置将归结为
var finalvelocity = 27.326 + 0*0;
var finalvelocity == 27.326
如果finalvelocity成为currentlocation的速度,那么previouslocation=currentlocation。这就意味着你的最终位置可能不会下降。但是,这里有很多假设。根据我们的讨论,由于你的加速度在不断变化,你应用的运动方程不会给你一个准确的答案 当你获得新的加速度读数时,你可能需要不断更新你的位置和速度
由于这将是非常低效的,我的建议是每隔几秒钟调用一次更新函数,并使用这段时间内加速度的平均值来获得新的速度和位置。正如你们中的一些人所提到的,你得到的方程式是错误的,但这只是错误的一部分
// init values
double ax=0.0,ay=0.0,az=0.0; // acceleration [m/s^2]
double vx=0.0,vy=0.0,vz=0.0; // velocity [m/s]
double x=0.0, y=0.0, z=0.0; // position [m]
// iteration inside some timer (dt [seconds] period) ...
ax,ay,az = accelerometer values
vx+=ax*dt; // update speed via integration of acceleration
vy+=ay*dt;
vz+=az*dt;
x+=vx*dt; // update position via integration of velocity
y+=vy*dt;
z+=vz*dt;
// init values
double gx=0.0,gy=-9.81,gz=0.0; // [edit1] background gravity in map coordinate system [m/s^2]
double ax=0.0,ay=0.0,az=0.0; // acceleration [m/s^2]
double vx=0.0,vy=0.0,vz=0.0; // velocity [m/s]
double x=0.0, y=0.0, z=0.0; // position [m]
double dev[9]; // actual device transform matrix ... local coordinate system
(x,y,z) <- GPS position;
// iteration inside some timer (dt [seconds] period) ...
dev <- compass direction
ax,ay,az = accelerometer values (measured in device space)
(ax,ay,az) = dev*(ax,ay,az); // transform acceleration from device space to global map space without any translation to preserve vector magnitude
ax-=gx; // [edit1] remove background gravity (in map coordinate system)
ay-=gy;
az-=gz;
vx+=ax*dt; // update speed (in map coordinate system)
vy+=ay*dt;
vz+=az*dt;
x+=vx*dt; // update position (in map coordinate system)
y+=vy*dt;
z+=vz*dt;
public static void PlotNewPosition(Double newLatitude, Double newLongitude, Float currentDistance,
Float currentAcceleration, Float currentSpeed, Float currentDirection, String dataType) {
LatLng newPosition = new LatLng(newLongitude,newLatitude);
if(dataType == "Sensor"){
tvAcceleration.setText("Speed: " + currentSpeed + " Acceleration: " + currentAcceleration + " Distance: " + currentDistance +" Direction: " + currentDirection + " \n");
map.addMarker(new MarkerOptions()
.position(newPosition)
.title("Position")
.snippet("Sensor Position")
.icon(BitmapDescriptorFactory
.fromResource(R.drawable.line)));
}else if(dataType == "GPSSensor"){
map.addMarker(new MarkerOptions()
.position(newPosition)
.title("PositionCollaborated")
.snippet("GPS Position"));
}
else{
map.addMarker(new MarkerOptions()
.position(newPosition)
.title("Position")
.snippet("New Position")
.icon(BitmapDescriptorFactory
.fromResource(R.drawable.linered)));
}
map.moveCamera(CameraUpdateFactory.newLatLngZoom(newPosition, 18));
}
// init values
double ax=0.0,ay=0.0,az=0.0; // acceleration [m/s^2]
double vx=0.0,vy=0.0,vz=0.0; // velocity [m/s]
double x=0.0, y=0.0, z=0.0; // position [m]
// iteration inside some timer (dt [seconds] period) ...
ax,ay,az = accelerometer values
vx+=ax*dt; // update speed via integration of acceleration
vy+=ay*dt;
vz+=az*dt;
x+=vx*dt; // update position via integration of velocity
y+=vy*dt;
z+=vz*dt;
// init values
double gx=0.0,gy=-9.81,gz=0.0; // [edit1] background gravity in map coordinate system [m/s^2]
double ax=0.0,ay=0.0,az=0.0; // acceleration [m/s^2]
double vx=0.0,vy=0.0,vz=0.0; // velocity [m/s]
double x=0.0, y=0.0, z=0.0; // position [m]
double dev[9]; // actual device transform matrix ... local coordinate system
(x,y,z) <- GPS position;
// iteration inside some timer (dt [seconds] period) ...
dev <- compass direction
ax,ay,az = accelerometer values (measured in device space)
(ax,ay,az) = dev*(ax,ay,az); // transform acceleration from device space to global map space without any translation to preserve vector magnitude
ax-=gx; // [edit1] remove background gravity (in map coordinate system)
ay-=gy;
az-=gz;
vx+=ax*dt; // update speed (in map coordinate system)
vy+=ay*dt;
vz+=az*dt;
x+=vx*dt; // update position (in map coordinate system)
y+=vy*dt;
z+=vz*dt;
public static void PlotNewPosition(Double newLatitude, Double newLongitude, Float currentDistance,
Float currentAcceleration, Float currentSpeed, Float currentDirection, String dataType) {
LatLng newPosition = new LatLng(newLongitude,newLatitude);
if(dataType == "Sensor"){
tvAcceleration.setText("Speed: " + currentSpeed + " Acceleration: " + currentAcceleration + " Distance: " + currentDistance +" Direction: " + currentDirection + " \n");
map.addMarker(new MarkerOptions()
.position(newPosition)
.title("Position")
.snippet("Sensor Position")
.icon(BitmapDescriptorFactory
.fromResource(R.drawable.line)));
}else if(dataType == "GPSSensor"){
map.addMarker(new MarkerOptions()
.position(newPosition)
.title("PositionCollaborated")
.snippet("GPS Position"));
}
else{
map.addMarker(new MarkerOptions()
.position(newPosition)
.title("Position")
.snippet("New Position")
.icon(BitmapDescriptorFactory
.fromResource(R.drawable.linered)));
}
map.moveCamera(CameraUpdateFactory.newLatLngZoom(newPosition, 18));
}
//初始化值
双gx=0.0,gy=-9.81,gz=0.0;//[edit1]地图坐标系中的背景重力[m/s^2]
双ax=0.0,ay=0.0,az=0.0;//加速度[m/s^2]
双vx=0.0,vy=0.0,vz=0.0;//速度[米/秒]
双x=0.0,y=0.0,z=0.0;//位置[m]
双dev[9];//实际设备转换矩阵。。。局部坐标系
(x,y,z)加速度传感器和陀螺仪不适用于位置计算。
几秒钟后,误差变得难以置信的高。(我几乎不记得双重整合是个问题)。
看看这个关于传感器熔断的例子,
他非常详细地解释了为什么这是不可能的。解决了我使用传感器计算的位置后,我想在这里发布我的代码,以防将来有人需要:
注意:这只在三星Galaxy S2手机上进行了检查,只有当人们带着手机走路时,才进行了检查,而在汽车或自行车上移动时未进行测试
这是我在与GPS比较时得到的结果(红线GPS,蓝色是用传感器计算的位置)
代码不是很有效,但我希望我分享的代码能帮助别人,为他们指明正确的方向
我有两门不同的课:
计算位置
客户传感器服务
公共类计算位置{
static Double earthRadius = 6378D;
static Double oldLatitude,oldLongitude;
static Boolean IsFirst = true;
static Double sensorLatitude, sensorLongitude;
static Date CollaborationWithGPSTime;
public static float[] results;
public static void calculateNewPosition(Context applicationContext,
Float currentAcceleration, Float currentSpeed,
Float currentDistanceTravelled, Float currentDirection, Float TotalDistance) {
results = new float[3];
if(IsFirst){
CollaborationWithGPSTime = new Date();
Toast.makeText(applicationContext, "First", Toast.LENGTH_LONG).show();
oldLatitude = CustomLocationListener.mLatitude;
oldLongitude = CustomLocationListener.mLongitude;
sensorLatitude = oldLatitude;
sensorLongitude = oldLongitude;
LivePositionActivity.PlotNewPosition(oldLongitude,oldLatitude,currentDistanceTravelled * 1000, currentAcceleration, currentSpeed, currentDirection, "GPSSensor",0.0F,TotalDistance);
IsFirst = false;
return;
}
Date CurrentDateTime = new Date();
if(CurrentDateTime.getTime() - CollaborationWithGPSTime.getTime() > 900000){
//This IF Statement is to Collaborate with GPS position --> For accuracy --> 900,000 == 15 minutes
oldLatitude = CustomLocationListener.mLatitude;
oldLongitude = CustomLocationListener.mLongitude;
LivePositionActivity.PlotNewPosition(oldLongitude,oldLatitude,currentDistanceTravelled * 1000, currentAcceleration, currentSpeed, currentDirection, "GPSSensor", 0.0F, 0.0F);
return;
}
//Convert Variables to Radian for the Formula
oldLatitude = Math.PI * oldLatitude / 180;
oldLongitude = Math.PI * oldLongitude / 180;
currentDirection = (float) (Math.PI * currentDirection / 180.0);
//Formulae to Calculate the NewLAtitude and NewLongtiude
Double newLatitude = Math.asin(Math.sin(oldLatitude) * Math.cos(currentDistanceTravelled / earthRadius) +
Math.cos(oldLatitude) * Math.sin(currentDistanceTravelled / earthRadius) * Math.cos(currentDirection));
Double newLongitude = oldLongitude + Math.atan2(Math.sin(currentDirection) * Math.sin(currentDistanceTravelled / earthRadius)
* Math.cos(oldLatitude), Math.cos(currentDistanceTravelled / earthRadius)
- Math.sin(oldLatitude) * Math.sin(newLatitude));
//Convert Back from radians
newLatitude = 180 * newLatitude / Math.PI;
newLongitude = 180 * newLongitude / Math.PI;
currentDirection = (float) (180 * currentDirection / Math.PI);
//Update old Latitude and Longitude
oldLatitude = newLatitude;
oldLongitude = newLongitude;
sensorLatitude = oldLatitude;
sensorLongitude = oldLongitude;
IsFirst = false;
//Plot Position on Map
LivePositionActivity.PlotNewPosition(newLongitude,newLatitude,currentDistanceTravelled * 1000, currentAcceleration, currentSpeed, currentDirection, "Sensor", results[0],TotalDistance);
}
}
公共类CustomSensorService扩展服务实现SensorEventListener{
static SensorManager sensorManager;
static Sensor mAccelerometer;
private Sensor mMagnetometer;
private Sensor mLinearAccelertion;
static Context mContext;
private static float[] AccelerometerValue;
private static float[] MagnetometerValue;
public static Float currentAcceleration = 0.0F;
public static Float currentDirection = 0.0F;
public static Float CurrentSpeed = 0.0F;
public static Float CurrentDistanceTravelled = 0.0F;
/*---------------------------------------------*/
float[] prevValues,speed;
float[] currentValues;
float prevTime, currentTime, changeTime,distanceY,distanceX,distanceZ;
float[] currentVelocity;
public static CalculatePosition CalcPosition;
/*-----FILTER VARIABLES-------------------------*-/
*
*
*/
public static Float prevAcceleration = 0.0F;
public static Float prevSpeed = 0.0F;
public static Float prevDistance = 0.0F;
public static Float totalDistance;
TextView tv;
Boolean First,FirstSensor = true;
@Override
public void onCreate(){
super.onCreate();
mContext = getApplicationContext();
CalcPosition = new CalculatePosition();
First = FirstSensor = true;
currentValues = new float[3];
prevValues = new float[3];
currentVelocity = new float[3];
speed = new float[3];
totalDistance = 0.0F;
Toast.makeText(getApplicationContext(),"Service Created",Toast.LENGTH_SHORT).show();
sensorManager = (SensorManager) getSystemService(SENSOR_SERVICE);
mAccelerometer = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
mMagnetometer = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
//mGyro = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
mLinearAccelertion = sensorManager.getDefaultSensor(Sensor.TYPE_LINEAR_ACCELERATION);
sensorManager.registerListener(this, mAccelerometer, SensorManager.SENSOR_DELAY_NORMAL);
sensorManager.registerListener(this, mMagnetometer, SensorManager.SENSOR_DELAY_NORMAL);
//sensorManager.registerListener(this, mGyro, SensorManager.SENSOR_DELAY_NORMAL);
sensorManager.registerListener(this, mLinearAccelertion, SensorManager.SENSOR_DELAY_NORMAL);
}
@Override
public void onDestroy(){
Toast.makeText(this, "Service Destroyed", Toast.LENGTH_SHORT).show();
sensorManager.unregisterListener(this);
//sensorManager = null;
super.onDestroy();
}
@Override
public void onAccuracyChanged(Sensor sensor, int accuracy) {
// TODO Auto-generated method stub
}
@Override
public void onSensorChanged(SensorEvent event) {
float[] values = event.values;
Sensor mSensor = event.sensor;
if(mSensor.getType() == Sensor.TYPE_ACCELEROMETER){
AccelerometerValue = values;
}
if(mSensor.getType() == Sensor.TYPE_LINEAR_ACCELERATION){
if(First){
prevValues = values;
prevTime = event.timestamp / 1000000000;
First = false;
currentVelocity[0] = currentVelocity[1] = currentVelocity[2] = 0;
distanceX = distanceY= distanceZ = 0;
}
else{
currentTime = event.timestamp / 1000000000.0f;
changeTime = currentTime - prevTime;
prevTime = currentTime;
calculateDistance(event.values, changeTime);
currentAcceleration = (float) Math.sqrt(event.values[0] * event.values[0] + event.values[1] * event.values[1] + event.values[2] * event.values[2]);
CurrentSpeed = (float) Math.sqrt(speed[0] * speed[0] + speed[1] * speed[1] + speed[2] * speed[2]);
CurrentDistanceTravelled = (float) Math.sqrt(distanceX * distanceX + distanceY * distanceY + distanceZ * distanceZ);
CurrentDistanceTravelled = CurrentDistanceTravelled / 1000;
if(FirstSensor){
prevAcceleration = currentAcceleration;
prevDistance = CurrentDistanceTravelled;
prevSpeed = CurrentSpeed;
FirstSensor = false;
}
prevValues = values;
}
}
if(mSensor.getType() == Sensor.TYPE_MAGNETIC_FIELD){
MagnetometerValue = values;
}
if(currentAcceleration != prevAcceleration || CurrentSpeed != prevSpeed || prevDistance != CurrentDistanceTravelled){
if(!FirstSensor)
totalDistance = totalDistance + CurrentDistanceTravelled * 1000;
if (AccelerometerValue != null && MagnetometerValue != null && currentAcceleration != null) {
//Direction
float RT[] = new float[9];
float I[] = new float[9];
boolean success = SensorManager.getRotationMatrix(RT, I, AccelerometerValue,
MagnetometerValue);
if (success) {
float orientation[] = new float[3];
SensorManager.getOrientation(RT, orientation);
float azimut = (float) Math.round(Math.toDegrees(orientation[0]));
currentDirection =(azimut+ 360) % 360;
if( CurrentSpeed > 0.2){
CalculatePosition.calculateNewPosition(getApplicationContext(),currentAcceleration,CurrentSpeed,CurrentDistanceTravelled,currentDirection,totalDistance);
}
}
prevAcceleration = currentAcceleration;
prevSpeed = CurrentSpeed;
prevDistance = CurrentDistanceTravelled;
}
}
}
@Override
public IBinder onBind(Intent arg0) {
// TODO Auto-generated method stub
return null;
}
public void calculateDistance (float[] acceleration, float deltaTime) {
float[] distance = new float[acceleration.length];
for (int i = 0; i < acceleration.length; i++) {
speed[i] = acceleration[i] * deltaTime;
distance[i] = speed[i] * deltaTime + acceleration[i] * deltaTime * deltaTime / 2;
}
distanceX = distance[0];
distanceY = distance[1];
distanceZ = distance[2];
}
该应用程序将使用传感器而不是位置服务。传感器融合应用程序:-(好的观点。看起来我必须在这一点上对其进行过滤。可能是一个if条件。我在Android端添加了一些传感器过滤器。当我找到答案时,我会发布答案。我确实记住了你关于初始速度的观点。你确定运动方向和加速度在行驶过程中不会改变吗?你的方程式是什么应用假设在行驶过程中加速度保持不变。加速度变化。在^2处(从点A到点B)的s=ut+(1/2)等恒等式仅当加速度在a和B之间的整个时间内保持恒定在“a”时才能应用。那么,如果加速度在变化,该怎么办?加速度是如何变化的?当加速度在特定水平上保持恒定时,必须在短时间间隔内应用此标识。如果加速度是连续的以一定的速度递增/递减,那么你就进入了微积分的领域。很好的解释。你建议我在智能手机上自己做这些计算吗?只是为了弄清楚,dt=t2-t1,其中t1是初始时间,t2是读取时间?是的,dt是两次迭代之间的时间=实际时间-最后一次实际时间。如果是在迭代之前或之后进行测量(没有分支,但我希望在迭代开始时进行测量)感谢您的帮助。我只是想问一下,为什么我必须将以前的速度添加到新速度中?vx+=ax*dt;因为您只测量加速度…所以速度是积分的(通过该加法)…这只是积分计算的矩形法则是的…重力