NMEA日志文件中两个横向坐标和长坐标之间的c#计算器轴承？

从GPS日志中提取以下内容：

$GPGGA,153500.009,5137.2603,N,00244.8715,W,1,10,0.8,50.6,M,51.4,M,,0000*71
$GPRMC,153500.009,A,5137.2603,N,00244.8715,W,037.7,101.7,300912,,,A*74
$GPGGA,153500.059,5137.2601,N,00244.8706,W,1,10,0.8,50.6,M,51.4,M,,0000*74
$GPRMC,153500.059,A,5137.2601,N,00244.8706,W,038.0,101.8,300912,,,A*76
$GPGGA,153500.109,5137.2600,N,00244.8697,W,1,10,0.8,50.6,M,51.4,M,,0000*78
$GPRMC,153500.109,A,5137.2600,N,00244.8697,W,038.3,101.9,300912,,,A*78
$GPGGA,153500.159,5137.2599,N,00244.8688,W,1,10,0.8,50.5,M,51.4,M,,0000*73
$GPRMC,153500.159,A,5137.2599,N,00244.8688,W,038.6,101.9,300912,,,A*75
$GPGGA,153500.209,5137.2597,N,00244.8679,W,1,10,0.8,50.5,M,51.4,M,,0000*75
$GPRMC,153500.209,A,5137.2597,N,00244.8679,W,038.9,102.0,300912,,,A*76

我将记录的GPS方位角与计算的上一个和当前位置之间的方位角进行比较，并使用以下代码循环通过每条线：

string[] splitline = line.Split(',');
course = Convert.ToDouble(splitline[8]);
Lat = Convert.ToDouble(splitline[3]);
Long = Convert.ToDouble(splitline[5]);

LatDeg = (Convert.ToInt16(Lat) / 100) + (Lat - (Convert.ToInt16(Lat) / 100) * 100) / 60;
LongDeg = (Convert.ToInt16(Long) / 100) + (Long - (Convert.ToInt16(Long) / 100) * 100) / 60;
lastLatDeg = (Convert.ToInt16(lastLat) / 100) + (lastLat - (Convert.ToInt16(lastLat) / 100) * 100) / 60;
lastLongDeg = (Convert.ToInt16(lastLong) / 100) + (lastLong - (Convert.ToInt16(lastLong) / 100) * 100) / 60;

var dLon = lastLongDeg - LongDeg;
var y = Math.Sin(dLon) * Math.Cos(lastLatDeg);
var x = Math.Cos(lastLatDeg) * Math.Sin(LatDeg) - Math.Sin(lastLatDeg) *           Math.Cos(LatDeg) * Math.Cos(dLon);
Console.WriteLine(DEG_PER_RAD * Math.Atan2(y, x));
Console.WriteLine("> " + course + " <");

lastLat = Lat;
lastLong = Long;
lastcourse = course;

string[]splitline=line.Split（'，'）；
course=Convert.ToDouble（分割线[8]）；
Lat=转换为双（分割线[3]）；
Long=Convert.ToDouble（分割线[5]）；
LatDeg=（转换为16（Lat）/100）+（Lat-（转换为16（Lat）/100）*100）/60；
LongDeg=（转换为16（长）/100）+（长-（转换为16（长）/100）*100）/60；
lastLatDeg=（转换为16（lastLat）/100）+（lastLat-（转换为16（lastLat）/100）*100）/60；
lastLongDeg=（转换为16（lastLong）/100）+（lastLong-（转换为16（lastLong）/100）*100）/60；
var dLon=lastLongDeg-LongDeg；
变量y=数学Sin（dLon）*数学Cos（lastLatDeg）；
var x=数学余弦（lastLatDeg）*数学余弦（LatDeg）-数学余弦（lastLatDeg）*数学余弦（LatDeg）*数学余弦（dLon）；
控制台写入线（度/弧度*数学坐标2（y，x））；
Console.WriteLine（“>”+course+“我在代码中发现了一些问题，首先在解释纬度和经度时，您应该查看位置位于地球的哪个象限，并将其转换为负数（对于南部或西部位置）：

Lat = Convert.ToDouble(splitline[3]);
if (splitline[4] == "S")
    Lat = 0.0 - Lat;
Long = Convert.ToDouble(splitline[5]);
if (splitline[6] == "W")
    Long = 0.0 - Long;

其余的问题源于将度而不是弧度传递给数学函数，而经度增量的计算似乎相反。我介绍了几个辅助函数，并将该部分代码重写如下：

public static double DegreesToRadians(double degrees)
{
    return degrees * (Math.PI / 180);
}

public static double RadiansToDegrees(double radians)
{
    return radians * 180 / Math.PI;
}

double dLon = DegreesToRadians(LongDeg - lastLongDeg);
double y = Math.Sin(dLon) * Math.Cos(DegreesToRadians(lastLatDeg));
double x = Math.Cos(DegreesToRadians(lastLatDeg)) * Math.Sin(DegreesToRadians(LatDeg)) - Math.Sin(DegreesToRadians(lastLatDeg)) * Math.Cos(DegreesToRadians(LatDeg)) * Math.Cos(dLon);
Console.WriteLine((RadiansToDegrees(Math.Atan2(y, x)) + 360.0) % 360);
Console.WriteLine("> " + course + " <");

公共静态双度弧度（双度）
{
返回度*（数学PI/180）；
}
公共静态双弧度（双弧度）
{
返回弧度*180/Math.PI；
}
双dLon=度弧度（LongDeg-lastLongDeg）；
双y=数学Sin（dLon）*数学Cos（度数弧度（lastLatDeg））；
double x=数学Cos（度数弧度（lastLatDeg））*数学Sin（度数弧度（LatDeg））-数学Sin（度数弧度（lastLatDeg））*数学Cos（度数弧度（LatDeg））*数学Cos（dLon）；
控制台写入线（（弧度为2（y，x））+360.0）%360）；
Console.WriteLine（“>”+课程+”为什么要转换为整数。若要获得精确的数字，请将结果保留为双精度并除以100.00。您可以在使用math.Round执行数学运算后截断最终结果。唯一的原因是将原始lat-long格式拆分为int：5137.2597=5137，然后将其除以100=51，即GPRMC lat-long-format=51deg 37.2597分钟和此nee将ds转换为十进制度数。多亏了PeterJ，我的工作方式符合我的要求，我认为我需要查看平滑数据或每2次读取一次，因为在10hz下，小距离可能会导致错误，例如在0.1秒内从109度翻转到100度会形成波浪线。
public static double DegreesToRadians(double degrees)
{
    return degrees * (Math.PI / 180);
}

public static double RadiansToDegrees(double radians)
{
    return radians * 180 / Math.PI;
}

double dLon = DegreesToRadians(LongDeg - lastLongDeg);
double y = Math.Sin(dLon) * Math.Cos(DegreesToRadians(lastLatDeg));
double x = Math.Cos(DegreesToRadians(lastLatDeg)) * Math.Sin(DegreesToRadians(LatDeg)) - Math.Sin(DegreesToRadians(lastLatDeg)) * Math.Cos(DegreesToRadians(LatDeg)) * Math.Cos(dLon);
Console.WriteLine((RadiansToDegrees(Math.Atan2(y, x)) + 360.0) % 360);
Console.WriteLine("> " + course + " <");

109.693614586392
> 101.8 <
100.14641169874
> 101.9 <
100.146411372034
> 101.9 <
109.693611985053
> 102 <