偏航率計算來自 GPS Heading

Question

我有一個雙 GPS 安裝在機器人上並以 20Hz 的頻率獲取數據。 我從 GNTRA 句子中獲得了航向數據，它給出了雙天線的航向。 我正在嘗試獲得車輛的偏航率。 當我轉向北方的左側時，我遇到了問題。 通常，我試圖獲得兩次測量的差異並將其除以時間間隔。 我不擅長三角函數。 例如，當我駕駛到 5 度航向並向左轉 350 度時。我需要測量 -15 度/time_interval。 但是我正在用我的代碼測量 345/time_interval。 我犯了錯誤，我不知道如何解決。 有誰能夠幫我？ 謝謝

        public double calc_yawrate(heading)
        {
         mywatch.Stop();
         double t = (double)mywatch.ElapsedMilliseconds / 1000;
         mywatch.Reset();
         mywatch.Start();


         Speed = Speed / 3.6;
         Speed = Math.Round(Speed, 2);
         double yaw = heading;
         double yawrate;
         yawrate = (yaw- yawpast) / t;
         yawpast = yaw;
        }

Answer 1

看起來你想要范圍 [-180°, 180°] 的差異，然后你可以將值加/減 360° 直到它在范圍內。

var diff = yaw - yawpast;

if(diff > 180)
{
    do
    {
        diff -= 360;
    }
    while(diff <= 180);
}
else if(diff < -180)
{
    do
    {
        diff += 360;
    }
    while(diff >= -180);
}

yawrate = diff / t;