我的计算智能手机位置的算法 - GPS 和传感器

问题描述

我正在开发一个基于传感器数据计算位置的安卓应用

I am developing an android application to calculate position based on Sensor's Data

1. 加速度计 --> 计算线性加速度

1. Accelerometer --> Calculate Linear Acceleration

磁力计 + 加速度计 --> 运动方向

Magnetometer + Accelerometer --> Direction of movement

初始位置将从 GPS(纬度 + 经度)获取.

The initial position will be taken from GPS (Latitude + Longitude).

现在根据传感器的读数，我需要计算智能手机的新位置:

Now based on Sensor's Readings i need to calculate the new position of the Smartphone:

我的算法如下 -(但没有计算准确位置):请帮助我改进它.

My Algorithm is following - (But is not calculating Accurate Position): Please help me improve it.

注意: 我的算法代码是 C#(我将传感器数据发送到服务器 - 数据存储在数据库中的位置.我正在计算服务器上的位置)

所有日期时间对象都使用时间戳计算 - 从 01-01-1970

    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.3263111114502 所以计算速度有问题，但我不知道是什么

This is the Result I get --> Phone was not moving. As you can see speed is 27.3263111114502 So there is something wrong in calculating Speed but I don't know what

答案:

我找到了一种基于传感器计算位置的解决方案:我在下面发布了一个答案.

I found a solution to calculate position based on Sensor: I have posted an Answer below.

如果您需要任何帮助，请发表评论

If you need any help, please leave a comment

这是与 GPS 相比的结果(注意: GPS 为红色)

this is The results compared to GPS (Note: GPS is in Red)

推荐答案

正如你们中的一些人提到的，你的方程有误，但这只是错误的一部分.

As some of you mentioned you got the equations wrong but that is just a part of the error.

1. 牛顿 - 非相对论速度的达朗贝尔物理学说明了这一点:

// 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;

• gx,gy,gz 是全球重力矢量(地球上的~9.81 m/s^2)
• 在代码中我的全局 Y 轴指向上，所以 gy=-9.81 和其余的是 0.0
• gx,gy,gz is the global gravity vector (~9.81 m/s^2 on Earth)
• in code my global Y axis points up so the gy=-9.81 and the rest are 0.0

衡量时机很关键

必须尽可能经常检查加速度计(秒是很长的时间).我建议不要使用大于 10 ms 的计时器周期来保持准确性，并且您应该不时使用 GPS 值覆盖计算的位置.可以不经常检查罗盘方向，但要进行适当的过滤

Accelerometer must be checked as often as possible (second is a very long time). I recommend not to use timer period bigger than 10 ms to preserve accuracy also time to time you should override calculated position with GPS value. Compass direction can be checked less often but with proper filtration

指南针不是一直正确

应该过滤某些峰值的罗盘值.有时它读取错误的值，也可能被电磁污染或金属环境关闭.在这种情况下，可以在移动过程中通过 GPS 检查方向并进行一些更正.例如每分钟检查一次 GPS 并将 GPS 方向与指南针进行比较，如果它在某个角度不断变化，则添加或减去它.

Compass values should be filtered for some peak values. Sometimes it read bad values and also can be off by electro-magnetic polution or metal enviroment. In that case the direction can be checked by GPS during movement and some corrections can be made. For example chech GPS every minute and compare GPS direction with compass and if it is constantly of by some angle then add it or substract it.

为什么要在服务器上做简单的计算???

讨厌在线浪费流量.是的，您可以在服务器上记录数据(但我仍然认为设备上的文件会更好)但是为什么要通过互联网连接来限制位置功能???更不用说延迟了......

Hate on-line waste of traffic. Yes you can log data on server (but still i think file on device will be better) but why to heck limit position functionality by internet connection ??? not to mention the delays ...

附加注释

稍微编辑了上面的代码.方向必须尽可能精确，以最大限度地减少累积误差.

Edited the code above a little. The orientation must be as precise as it can be to minimize cumulative errors.

陀螺仪会比指南针更好(或者甚至更好地使用它们).应过滤加速度.一些低通滤波应该没问题.去除重力后，我会将 ax,ay,az 限制为可用值并丢弃太小的值.如果接近低速也做完全停止(如果它不是火车或真空运动).这应该会降低漂移但会增加其他误差，因此必须在它们之间找到折衷方案.

Gyros would be better than compass (or even better use them both). Acceleration should be filtered. Some low pass filtering should be OK. After gravity removal I would limit ax,ay,az to usable values and throw away too small values. If near low speed also do full stop (if it is not a train or motion in vacuum). That should lower the drift but increase other errors so an compromise has to be found between them.

即时添加校准.当过滤 acceleration = 9.81 或非常接近它时，则设备可能处于静止状态(除非它是飞行器).方向/方向可通过实际重力方向进行修正.

Add calibration on the fly. When filtered acceleration = 9.81 or very close to it then the device is probably stand still (unless its a flying machine). Orientation/direction can be corrected by actual gravity direction.

这篇关于我的计算智能手机位置的算法 - GPS 和传感器的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持！