diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc
index e980718984b1b7aa8f85811a299e7791d74d4907..c14efdda56247f37b948f73010a5bc09ff5f7b04 100644
--- a/src/comm/MAVLinkSimulationMAV.cc
+++ b/src/comm/MAVLinkSimulationMAV.cc
@@ -70,16 +70,16 @@ void MAVLinkSimulationMAV::mainloop()
     // 10 Hz execution
     if (timer10Hz <= 0)
     {
-        if (!firstWP)
-        {
-            double radPer100ms = 0.00006;
-            double altPer100ms = 1.0;
-            double xm = (nextSPX - x);
-            double ym = (nextSPY - y);
-            double zm = (nextSPZ - z);
+        double radPer100ms = 0.00006;
+        double altPer100ms = 0.4;
+        double xm = (nextSPX - x);
+        double ym = (nextSPY - y);
+        double zm = (nextSPZ - z);
 
-            float zsign = (zm < 0) ? -1.0f : 1.0f;
+        float zsign = (zm < 0) ? -1.0f : 1.0f;
 
+        if (!firstWP)
+        {
             //float trueyaw = atan2f(xm, ym);
 
             float newYaw = atan2f(xm, ym);
@@ -121,9 +121,9 @@ void MAVLinkSimulationMAV::mainloop()
         pos.alt = z*1000.0;
         pos.lat = y*1E7;
         pos.lon = x*1E7;
-        pos.vx = sin(yaw)*10.0f;
-        pos.vy = cos(yaw)*10.0f;
-        pos.vz = 0;
+        pos.vx = sin(yaw)*10.0f*100.0f;
+        pos.vy = cos(yaw)*10.0f*100.0f;
+        pos.vz = altPer100ms*10.0f*100.0f*zsign*-1.0f;
         mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos);
         link->sendMAVLinkMessage(&msg);
         planner.handleMessage(msg);
diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc
index c6785db7f1a643e6785e97e31a99bdc25622dcf4..9726673b93db615170bcedfb887e05cf0bb1e83c 100644
--- a/src/ui/HUD.cc
+++ b/src/ui/HUD.cc
@@ -37,9 +37,7 @@ This file is part of the QGROUNDCONTROL project
 
 #include <QDebug>
 #include <cmath>
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
+#include <qmath.h>
 #include <limits>
 
 #include "UASManager.h"
@@ -126,6 +124,9 @@ HUD::HUD(int width, int height, QWidget* parent)
     xSpeed(0.0),
     ySpeed(0.0),
     zSpeed(0.0),
+    lastSpeedUpdate(0),
+    totalSpeed(0.0),
+    totalAcc(0.0),
     lat(0.0),
     lon(0.0),
     alt(0.0),
@@ -277,6 +278,7 @@ void HUD::setActiveUAS(UASInterface* uas)
         disconnect(this->uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*)));
 
         disconnect(this->uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
+        disconnect(this->uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
         disconnect(this->uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
         disconnect(this->uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int)));
 
@@ -299,6 +301,7 @@ void HUD::setActiveUAS(UASInterface* uas)
         connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*)));
 
         connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
+        connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64)));
         connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64)));
         connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int, int)));
 
@@ -334,12 +337,7 @@ void HUD::updateAttitude(UASInterface* uas, double roll, double pitch, double ya
 void HUD::updateBattery(UASInterface* uas, double voltage, double percent, int seconds)
 {
     Q_UNUSED(uas);
-//    this->voltage = voltage;
-//    this->timeRemaining = seconds;
-//    this->percentRemaining = percent;
-
-    fuelStatus.sprintf("BAT [%02.0f % | %05.2fV] (%02dm:%02ds)", percent, voltage, seconds/60, seconds%60);
-
+    fuelStatus = tr("BAT [%1% | %2V] (%3:%4)").arg(percent, 2, 'f', 0, QChar('0')).arg(voltage, 4, 'f', 1, QChar('0')).arg(seconds/60, 2, 10, QChar('0')).arg(seconds%60, 2, 10, QChar('0'));
     if (percent < 20.0f)
     {
         fuelColor = warningColor;
@@ -390,6 +388,9 @@ void HUD::updateSpeed(UASInterface* uas,double x,double y,double z,quint64 times
     this->xSpeed = x;
     this->ySpeed = y;
     this->zSpeed = z;
+    double newTotalSpeed = sqrt(xSpeed*xSpeed + ySpeed*ySpeed + zSpeed*zSpeed);
+    totalAcc = (newTotalSpeed - totalSpeed) / ((double)(lastSpeedUpdate - timestamp)/1000.0);
+    totalSpeed = newTotalSpeed;
 }
 
 /**
@@ -814,15 +815,26 @@ void HUD::paintHUD()
             drawChangeRateStrip(-51.0f, -50.0f, 15.0f, -1.0f, 1.0f, -zSpeed, &painter);
 
             // CHANGE RATE STRIPS
-            drawChangeRateStrip(49.0f, -50.0f, 15.0f, -1.0f, 1.0f, xSpeed, &painter);
+            drawChangeRateStrip(49.0f, -50.0f, 15.0f, -1.0f, 1.0f, totalAcc, &painter);
 
             // GAUGES
 
             // Left altitude gauge
-            drawChangeIndicatorGauge(-vGaugeSpacing, -15.0f, 10.0f, 2.0f, -zPos, defaultColor, &painter, false);
+            float gaugeAltitude;
+
+            if (this->alt != 0)
+            {
+                gaugeAltitude = alt;
+            }
+            else
+            {
+                gaugeAltitude = -zPos;
+            }
+
+            drawChangeIndicatorGauge(-vGaugeSpacing, -15.0f, 10.0f, 2.0f, gaugeAltitude, defaultColor, &painter, false);
 
             // Right speed gauge
-            drawChangeIndicatorGauge(vGaugeSpacing, -15.0f, 10.0f, 5.0f, xSpeed, defaultColor, &painter, false);
+            drawChangeIndicatorGauge(vGaugeSpacing, -15.0f, 10.0f, 5.0f, totalSpeed, defaultColor, &painter, false);
 
 
             // Waypoint name
diff --git a/src/ui/HUD.h b/src/ui/HUD.h
index 8866f784abd4ee73692e34aefa922999831cb7c0..b476fac5dc45d076f79bbc9925b9ded1d22e4fd9 100644
--- a/src/ui/HUD.h
+++ b/src/ui/HUD.h
@@ -130,7 +130,7 @@ protected:
     void contextMenuEvent (QContextMenuEvent* event);
     void createActions();
 
-    static const int updateInterval = 50;
+    static const int updateInterval = 40;
 
     QImage* image; ///< Double buffer image
     QImage glImage; ///< The background / camera image
@@ -198,6 +198,9 @@ protected:
     double xSpeed;
     double ySpeed;
     double zSpeed;
+    quint64 lastSpeedUpdate;
+    double totalSpeed;
+    double totalAcc;
     double lat;
     double lon;
     double alt;