diff --git a/SourceCode/GPS/Classes/CAngleSensor.cs b/SourceCode/GPS/Classes/CAngleSensor.cs
new file mode 100644
index 0000000..bd0037a
--- /dev/null
+++ b/SourceCode/GPS/Classes/CAngleSensor.cs
@@ -0,0 +1,189 @@
+//Please, if you use this, share the improvements
+
+using System;
+using System.Globalization;
+
+namespace OpenGrade
+{
+    /// <summary>
+    /// Handles angle sensor input and filtering for excavator arm and stick
+    /// Compensates for movement of arm and stick affecting GPS antenna reading
+    /// </summary>
+    public class CAngleSensor
+    {
+        // Angle sensor data
+        public double armAngle; // Angle of main boom arm (degrees)
+        public double stickAngle; // Angle of bucket stick (degrees)
+        
+        // Filtered angle values using moving average
+        private double[] armAngleBuffer;
+        private double[] stickAngleBuffer;
+        private int bufferIndex = 0;
+        private const int FILTER_BUFFER_SIZE = 10;
+        
+        // Raw sensor values
+        public double armAngleRaw;
+        public double stickAngleRaw;
+        
+        // Calibration offsets
+        public double armAngleOffset = 0.0;
+        public double stickAngleOffset = 0.0;
+        
+        // Sensor connection status
+        public bool isConnected = false;
+        public string lastError = "";
+        
+        // Update timestamp
+        public DateTime lastUpdate;
+        
+        public CAngleSensor()
+        {
+            armAngleBuffer = new double[FILTER_BUFFER_SIZE];
+            stickAngleBuffer = new double[FILTER_BUFFER_SIZE];
+            
+            // Initialize buffers
+            for (int i = 0; i < FILTER_BUFFER_SIZE; i++)
+            {
+                armAngleBuffer[i] = 0.0;
+                stickAngleBuffer[i] = 0.0;
+            }
+            
+            lastUpdate = DateTime.Now;
+        }
+        
+        /// <summary>
+        /// Parse angle sensor data from serial input
+        /// Expected format: $ANGLE,armAngle,stickAngle*checksum
+        /// Example: $ANGLE,45.2,32.5*2F
+        /// </summary>
+        public bool ParseAngleData(string sentence)
+        {
+            try
+            {
+                if (string.IsNullOrEmpty(sentence) || !sentence.StartsWith("$ANGLE"))
+                    return false;
+                
+                // Remove checksum
+                int checksumIndex = sentence.IndexOf('*');
+                if (checksumIndex > 0)
+                    sentence = sentence.Substring(0, checksumIndex);
+                
+                // Split by comma
+                string[] parts = sentence.Split(',');
+                if (parts.Length < 3)
+                {
+                    lastError = "Invalid angle sensor format";
+                    return false;
+                }
+                
+                // Parse arm angle
+                if (!double.TryParse(parts[1], NumberStyles.Float, CultureInfo.InvariantCulture, out armAngleRaw))
+                {
+                    lastError = "Failed to parse arm angle";
+                    return false;
+                }
+                
+                // Parse stick angle
+                if (!double.TryParse(parts[2], NumberStyles.Float, CultureInfo.InvariantCulture, out stickAngleRaw))
+                {
+                    lastError = "Failed to parse stick angle";
+                    return false;
+                }
+                
+                // Apply calibration offsets
+                armAngle = armAngleRaw - armAngleOffset;
+                stickAngle = stickAngleRaw - stickAngleOffset;
+                
+                // Apply filtering
+                UpdateFilteredValues();
+                
+                lastUpdate = DateTime.Now;
+                isConnected = true;
+                lastError = "";
+                
+                return true;
+            }
+            catch (Exception e)
+            {
+                lastError = "Parse error: " + e.Message;
+                return false;
+            }
+        }
+        
+        /// <summary>
+        /// Update filtered angle values using moving average
+        /// </summary>
+        private void UpdateFilteredValues()
+        {
+            armAngleBuffer[bufferIndex] = armAngle;
+            stickAngleBuffer[bufferIndex] = stickAngle;
+            
+            bufferIndex++;
+            if (bufferIndex >= FILTER_BUFFER_SIZE)
+                bufferIndex = 0;
+            
+            // Calculate moving averages
+            double armSum = 0;
+            double stickSum = 0;
+            
+            for (int i = 0; i < FILTER_BUFFER_SIZE; i++)
+            {
+                armSum += armAngleBuffer[i];
+                stickSum += stickAngleBuffer[i];
+            }
+            
+            armAngle = armSum / FILTER_BUFFER_SIZE;
+            stickAngle = stickSum / FILTER_BUFFER_SIZE;
+        }
+        
+        /// <summary>
+        /// Calibrate angle sensors (call when arm/stick in known position)
+        /// </summary>
+        public void CalibrateAngles(double armKnownAngle, double stickKnownAngle)
+        {
+            armAngleOffset = armAngleRaw - armKnownAngle;
+            stickAngleOffset = stickAngleRaw - stickKnownAngle;
+        }
+        
+        /// <summary>
+        /// Normalize angles to 0-360 range
+        /// </summary>
+        public double NormalizeAngle(double angle)
+        {
+            angle = angle % 360.0;
+            if (angle < 0)
+                angle += 360.0;
+            return angle;
+        }
+        
+        /// <summary>
+        /// Convert degrees to radians
+        /// </summary>
+        public double DegreesToRadians(double degrees)
+        {
+            return degrees * Math.PI / 180.0;
+        }
+        
+        /// <summary>
+        /// Convert radians to degrees
+        /// </summary>
+        public double RadiansToDegrees(double radians)
+        {
+            return radians * 180.0 / Math.PI;
+        }
+        
+        /// <summary>
+        /// Check connection timeout (30 seconds)
+        /// </summary>
+        public bool IsConnectionAlive()
+        {
+            TimeSpan elapsed = DateTime.Now - lastUpdate;
+            if (elapsed.TotalSeconds > 30)
+            {
+                isConnected = false;
+                return false;
+            }
+            return true;
+        }
+    }
+}
diff --git a/SourceCode/GPS/Classes/CArmKinematics.cs b/SourceCode/GPS/Classes/CArmKinematics.cs
new file mode 100644
index 0000000..871e123
--- /dev/null
+++ b/SourceCode/GPS/Classes/CArmKinematics.cs
@@ -0,0 +1,179 @@
+//Please, if you use this, share the improvements
+
+using System;
+
+namespace OpenGrade
+{
+    /// <summary>
+    /// Calculates excavator arm kinematics to determine antenna position
+    /// relative to the excavator base, accounting for arm and stick angles
+    /// </summary>
+    public class CArmKinematics
+    {
+        // Excavator arm dimensions (in meters) - adjust for your specific machine
+        public double armLength = 5.0;        // Main boom length
+        public double stickLength = 3.0;      // Bucket stick length
+        
+        // Antenna offset from stick endpoint (in meters)
+        public double antennaOffsetX = 0.2;   // Forward offset
+        public double antennaOffsetY = 0.1;   // Vertical offset
+        public double antennaOffsetZ = 0.0;   // Side offset
+        
+        // Base reference point (where arm connects to excavator body)
+        public double baseHeight = 2.0;       // Height of arm pivot from ground
+        public double baseOffsetX = 0.5;      // Forward offset from center
+        public double baseOffsetY = 0.0;      // Side offset from center
+        
+        // Calculated antenna position relative to base
+        public vec3 antennaPosition;
+        
+        // Previous position for velocity calculation
+        private vec3 previousPosition;
+        private DateTime lastCalculation;
+        
+        public CArmKinematics()
+        {
+            antennaPosition = new vec3(0, 0, 0);
+            previousPosition = new vec3(0, 0, 0);
+            lastCalculation = DateTime.Now;
+        }
+        
+        /// <summary>
+        /// Calculate antenna position based on arm and stick angles
+        /// angles in degrees
+        /// returns vec3 with easting (X), northing (Y), altitude (Z) offsets from base
+        /// </summary>
+        public vec3 CalculateAntennaPosition(double armAngleDeg, double stickAngleDeg)
+        {
+            // Convert to radians
+            double armAngleRad = armAngleDeg * Math.PI / 180.0;
+            double stickAngleRad = stickAngleDeg * Math.PI / 180.0;
+            
+            // Calculate arm endpoint position (2D)
+            // Assuming 0 degrees = horizontal to the right, positive = upward
+            double armEndX = armLength * Math.Cos(armAngleRad);
+            double armEndY = armLength * Math.Sin(armAngleRad);
+            
+            // Calculate stick endpoint relative to arm endpoint
+            // Stick angle is relative to ground (not relative to arm)
+            double stickEndX = armEndX + stickLength * Math.Cos(stickAngleRad);
+            double stickEndY = armEndY + stickLength * Math.Sin(stickAngleRad);
+            
+            // Add antenna offset from stick endpoint
+            double antennaX = stickEndX + antennaOffsetX;
+            double antennaY = stickEndY + antennaOffsetY;
+            
+            // Add base offsets
+            double totalX = baseOffsetX + antennaX;
+            double totalY = baseHeight + antennaY;
+            double totalZ = baseOffsetY + antennaOffsetZ;
+            
+            // Store previous position
+            previousPosition = antennaPosition;
+            
+            // Update current position
+            antennaPosition = new vec3(totalX, totalY, totalZ);
+            lastCalculation = DateTime.Now;
+            
+            return antennaPosition;
+        }
+        
+        /// <summary>
+        /// Calculate antenna position with stick angle relative to arm
+        /// (alternative calculation method)
+        /// </summary>
+        public vec3 CalculateAntennaPositionRelative(double armAngleDeg, double stickAngleRelativeDeg)
+        {
+            double armAngleRad = armAngleDeg * Math.PI / 180.0;
+            double stickRelativeRad = stickAngleRelativeDeg * Math.PI / 180.0;
+            
+            // Arm endpoint
+            double armEndX = armLength * Math.Cos(armAngleRad);
+            double armEndY = armLength * Math.Sin(armAngleRad);
+            
+            // Stick angle relative to arm
+            double stickAbsoluteAngle = armAngleDeg + stickAngleRelativeDeg;
+            double stickAbsoluteRad = stickAbsoluteAngle * Math.PI / 180.0;
+            
+            // Stick endpoint
+            double stickEndX = armEndX + stickLength * Math.Cos(stickAbsoluteRad);
+            double stickEndY = armEndY + stickLength * Math.Sin(stickAbsoluteRad);
+            
+            // Add antenna offset
+            double antennaX = stickEndX + antennaOffsetX;
+            double antennaY = stickEndY + antennaOffsetY;
+            
+            previousPosition = antennaPosition;
+            antennaPosition = new vec3(baseOffsetX + antennaX, baseHeight + antennaY, baseOffsetY + antennaOffsetZ);
+            lastCalculation = DateTime.Now;
+            
+            return antennaPosition;
+        }
+        
+        /// <summary>
+        /// Get antenna velocity (change in position per second)
+        /// </summary>
+        public vec3 GetAntennaVelocity()
+        {
+            TimeSpan timeDelta = DateTime.Now - lastCalculation;
+            if (timeDelta.TotalSeconds < 0.001) // Avoid division by zero
+                return new vec3(0, 0, 0);
+            
+            double timeFactor = timeDelta.TotalSeconds;
+            double velX = (antennaPosition.easting - previousPosition.easting) / timeFactor;
+            double velY = (antennaPosition.northing - previousPosition.northing) / timeFactor;
+            double velZ = (antennaPosition.heading - previousPosition.heading) / timeFactor;
+            
+            return new vec3(velX, velY, velZ);
+        }
+        
+        /// <summary>
+        /// Get antenna displacement from previous calculation
+        /// </summary>
+        public double GetAntennaDisplacement()
+        {
+            double dx = antennaPosition.easting - previousPosition.easting;
+            double dy = antennaPosition.northing - previousPosition.northing;
+            double dz = antennaPosition.heading - previousPosition.heading;
+            
+            return Math.Sqrt(dx * dx + dy * dy + dz * dz);
+        }
+        
+        /// <summary>
+        /// Set arm dimensions for specific excavator model
+        /// </summary>
+        public void SetArmDimensions(double arm, double stick, double armBase, double stickBase)
+        {
+            armLength = arm;
+            stickLength = stick;
+            baseHeight = armBase;
+            baseOffsetX = stickBase;
+        }
+        
+        /// <summary>
+        /// Set antenna offset from bucket/stick endpoint
+        /// </summary>
+        public void SetAntennaOffset(double offsetX, double offsetY, double offsetZ)
+        {
+            antennaOffsetX = offsetX;
+            antennaOffsetY = offsetY;
+            antennaOffsetZ = offsetZ;
+        }
+        
+        /// <summary>
+        /// Validate arm angles are within physical limits
+        /// </summary>
+        public bool ValidateAngles(double armAngleDeg, double stickAngleDeg)
+        {
+            // Arm angle typically: -10 to 90 degrees
+            if (armAngleDeg < -20 || armAngleDeg > 100)
+                return false;
+            
+            // Stick angle typically: -90 to 30 degrees
+            if (stickAngleDeg < -120 || stickAngleDeg > 50)
+                return false;
+            
+            return true;
+        }
+    }
+}
diff --git a/SourceCode/GPS/Classes/CExcavatorVehicle.cs b/SourceCode/GPS/Classes/CExcavatorVehicle.cs
new file mode 100644
index 0000000..8c1d0a6
--- /dev/null
+++ b/SourceCode/GPS/Classes/CExcavatorVehicle.cs
@@ -0,0 +1,233 @@
+//Please, if you use this, share the improvements
+
+using System;
+using SharpGL;
+
+namespace OpenGrade
+{
+    /// <summary>
+    /// Excavator-specific vehicle class
+    /// Extends CVehicle functionality with angle sensor compensation
+    /// Calculates GPS antenna position based on arm and stick angles
+    /// </summary>
+    public class CExcavatorVehicle : CVehicle
+    {
+        private readonly OpenGL gl;
+        private readonly FormGPS mf;
+        
+        // Angle sensor and kinematics
+        public CAngleSensor angleSensor;
+        public CArmKinematics armKinematics;
+        
+        // Compensation settings
+        public bool enableAngleCompensation = true;
+        public bool enableFilteredAngles = true;
+        
+        // Compensated antenna position
+        private double compensatedEasting = 0;
+        private double compensatedNorthing = 0;
+        private double compensatedAltitude = 0;
+        
+        public CExcavatorVehicle(OpenGL _gl, FormGPS _f) : base(_gl, _f)
+        {
+            gl = _gl;
+            mf = _f;
+            
+            // Initialize angle sensor and kinematics
+            angleSensor = new CAngleSensor();
+            armKinematics = new CArmKinematics();
+            
+            // Load excavator-specific settings if available
+            LoadExcavatorSettings();
+        }
+        
+        /// <summary>
+        /// Load excavator-specific settings from properties
+        /// </summary>
+        private void LoadExcavatorSettings()
+        {
+            try
+            {
+                // Load arm dimensions
+                double armLen = Properties.Vehicle.Default.setExcavator_armLength;
+                double stickLen = Properties.Vehicle.Default.setExcavator_stickLength;
+                double armBase = Properties.Vehicle.Default.setExcavator_baseHeight;
+                double stickBase = Properties.Vehicle.Default.setExcavator_baseOffsetX;
+                
+                armKinematics.SetArmDimensions(armLen, stickLen, armBase, stickBase);
+                
+                // Load antenna offset
+                double offsetX = Properties.Vehicle.Default.setExcavator_antennaOffsetX;
+                double offsetY = Properties.Vehicle.Default.setExcavator_antennaOffsetY;
+                double offsetZ = Properties.Vehicle.Default.setExcavator_antennaOffsetZ;
+                
+                armKinematics.SetAntennaOffset(offsetX, offsetY, offsetZ);
+            }
+            catch
+            {
+                // Use defaults if properties not found
+            }
+        }
+        
+        /// <summary>
+        /// Update antenna position based on angle sensor readings
+        /// Call this when new angle sensor data is received
+        /// </summary>
+        public void UpdateAntennaPositionFromAngles()
+        {
+            if (!enableAngleCompensation || !angleSensor.isConnected)
+                return;
+            
+            // Calculate antenna position based on arm and stick angles
+            vec3 offset = armKinematics.CalculateAntennaPosition(
+                angleSensor.armAngle,
+                angleSensor.stickAngle);
+            
+            // Apply compensation to GPS position
+            // This adjusts the antenna reference point based on arm movement
+            compensatedEasting = offset.easting;
+            compensatedNorthing = offset.northing;
+            compensatedAltitude = offset.heading; // Using heading field for Z offset
+        }
+        
+        /// <summary>
+        /// Get the compensated GPS position
+        /// Applies antenna offset compensation based on arm/stick angles
+        /// </summary>
+        public vec3 GetCompensatedGPSPosition(vec3 baseGPSPosition)
+        {
+            if (!enableAngleCompensation || !angleSensor.isConnected)
+                return baseGPSPosition;
+            
+            // Apply compensation to base GPS position
+            return new vec3(
+                baseGPSPosition.easting + compensatedEasting,
+                baseGPSPosition.northing + compensatedNorthing,
+                baseGPSPosition.heading + compensatedAltitude);
+        }
+        
+        /// <summary>
+        /// Parse angle sensor data from serial input
+        /// </summary>
+        public bool ParseAngleSensorData(string sentence)
+        {
+            if (angleSensor.ParseAngleData(sentence))
+            {
+                UpdateAntennaPositionFromAngles();
+                return true;
+            }
+            return false;
+        }
+        
+        /// <summary>
+        /// Get antenna displacement from arm/stick movement
+        /// </summary>
+        public double GetAntennaDisplacement()
+        {
+            return armKinematics.GetAntennaDisplacement();
+        }
+        
+        /// <summary>
+        /// Check if angle sensor connection is active
+        /// </summary>
+        public bool IsAngleSensorActive()
+        {
+            return angleSensor.IsConnectionAlive();
+        }
+        
+        /// <summary>
+        /// Calibrate angle sensors to known position
+        /// </summary>
+        public void CalibrateAngleSensors(double armAngle, double stickAngle)
+        {
+            angleSensor.CalibrateAngles(armAngle, stickAngle);
+        }
+        
+        /// <summary>
+        /// Get current arm and stick angles
+        /// </summary>
+        public void GetCurrentAngles(out double armAngle, out double stickAngle)
+        {
+            armAngle = angleSensor.armAngle;
+            stickAngle = angleSensor.stickAngle;
+        }
+        
+        /// <summary>
+        /// Draw excavator arm based on current angles
+        /// </summary>
+        public void DrawExcavatorArm()
+        {
+            if (!angleSensor.isConnected)
+                return;
+            
+            gl.PushMatrix();
+            
+            // Translate to arm base
+            gl.Translate(
+                armKinematics.baseOffsetX,
+                armKinematics.baseHeight,
+                armKinematics.baseOffsetY);
+            
+            // Draw main arm boom
+            double armAngleRad = angleSensor.armAngle * Math.PI / 180.0;
+            gl.Color(0.7f, 0.7f, 0.7f);
+            gl.LineWidth(8.0f);
+            gl.Begin(OpenGL.GL_LINES);
+            gl.Vertex(0, 0, 0);
+            gl.Vertex(
+                (float)(armKinematics.armLength * Math.Cos(armAngleRad)),
+                (float)(armKinematics.armLength * Math.Sin(armAngleRad)),
+                0);
+            gl.End();
+            
+            // Translate to arm endpoint
+            gl.Translate(
+                (float)(armKinematics.armLength * Math.Cos(armAngleRad)),
+                (float)(armKinematics.armLength * Math.Sin(armAngleRad)),
+                0);
+            
+            // Draw bucket stick
+            double stickAngleRad = angleSensor.stickAngle * Math.PI / 180.0;
+            gl.Color(0.5f, 0.5f, 0.5f);
+            gl.LineWidth(6.0f);
+            gl.Begin(OpenGL.GL_LINES);
+            gl.Vertex(0, 0, 0);
+            gl.Vertex(
+                (float)(armKinematics.stickLength * Math.Cos(stickAngleRad)),
+                (float)(armKinematics.stickLength * Math.Sin(stickAngleRad)),
+                0);
+            gl.End();
+            
+            // Draw antenna position marker
+            gl.Translate(
+                (float)(armKinematics.stickLength * Math.Cos(stickAngleRad) + armKinematics.antennaOffsetX),
+                (float)(armKinematics.stickLength * Math.Sin(stickAngleRad) + armKinematics.antennaOffsetY),
+                (float)armKinematics.antennaOffsetZ);
+            
+            gl.Color(1.0f, 0.0f, 0.0f);
+            gl.PointSize(6.0f);
+            gl.Begin(OpenGL.GL_POINTS);
+            gl.Vertex(0, 0, 0);
+            gl.End();
+            
+            gl.PopMatrix();
+            gl.LineWidth(1);
+        }
+        
+        /// <summary>
+        /// Get status information for display
+        /// </summary>
+        public string GetStatusInfo()
+        {
+            if (!angleSensor.isConnected)
+                return "Angle Sensor: DISCONNECTED";
+            
+            return string.Format("Arm: {0:F1}°  Stick: {1:F1}°  Antenna Offset: ({2:F3}, {3:F3}, {4:F3})",
+                angleSensor.armAngle,
+                angleSensor.stickAngle,
+                compensatedEasting,
+                compensatedNorthing,
+                compensatedAltitude);
+        }
+    }
+}