EKF_code

import numpy as np
import pandas as pd
import gpxpy
import matplotlib.pyplot as plt

def extract_data_from_gpx(gpx_file_path):
    gpx = gpxpy.parse(open(gpx_file_path, 'r', encoding='utf-8'))

    # Extract data from GPX file
    data = []
    for track in gpx.tracks:
        for segment in track.segments:
            for point in segment.points:
                data.append([point.latitude, point.longitude, point.elevation, point.time, 0])  # Added speed as 0 initially

    # Calculate speed from distance and time
    distances = [point.distance_2d(previous_point) for point, previous_point in zip(segment.points[1:], segment.points[:-1])]
    times = [(point.time.timestamp() - previous_point.time.timestamp()) for point, previous_point in zip(segment.points[1:], segment.points[:-1])]
    speeds = [distance / time if time != 0 else 0 for distance, time in zip(distances, times)]
    speeds = [0] + speeds  # Assuming zero speed at the first point

    # Append speeds to data
    for i, point in enumerate(segment.points):
        data[i][4] = speeds[i]  # Update the speed in the existing data

    # Create DataFrame
    columns = ['latitude', 'longitude', 'elevation', 'time', 'speed']
    route_df = pd.DataFrame(data, columns=columns)

    return route_df

# ADD YOUR FILE PATH HERE
gpx_file_path = 'x'
route_df = extract_data_from_gpx(gpx_file_path)
#route_df = extract_data_from_gpx(gpx_file_path)
z_k = route_df[['latitude', 'longitude', 'elevation', 'time', 'speed']].values
# Add this print statement in your main function before the loop
print("Extracted GPX Data:")
print(z_k)


def ekf(z_k_observation_vector, state_estimate_k_minus_1, P_k_minus_1):
    # Update A matrix based on the bicycle model
    A_k_minus_1 = np.eye(3)  # Identity matrix

    # Define the process noise vector
    process_noise_v_k_minus_1 = np.array([0.01, 0.01, 0.003])

    # Define the state model noise covariance matrix Q_k
    Q_k = np.eye(3)  # Identity matrix

    # Define the measurement matrix H_k as an identity matrix
    H_k = np.eye(3)

    # Define the measurement noise covariance matrix R_k as identity matrix
    R_k = np.eye(3)

    # Predict Step
    state_estimate_k = A_k_minus_1 @ state_estimate_k_minus_1
    P_k = A_k_minus_1 @ P_k_minus_1 @ A_k_minus_1.T + Q_k

    # Measurement Residual
    measurement_residual_y_k = z_k_observation_vector - H_k @ state_estimate_k

    # Innovation Covariance
    S_k = H_k @ P_k @ H_k.T + R_k

    # Kalman Gain
    K_k = P_k @ H_k.T @ np.linalg.pinv(S_k)

    # Update Step
    state_estimate_k = state_estimate_k + K_k @ measurement_residual_y_k
    P_k = (np.eye(3) - K_k @ H_k) @ P_k

    return state_estimate_k, P_k


def main():
    # Extract data from GPX file
    # ADD YOUR FILE PATH HERE
    gpx_file_path = 'x'
    route_df = extract_data_from_gpx(gpx_file_path)
    z_k = route_df[['latitude', 'longitude', 'elevation']].values

    # Initialization
    # COPY AND PASTE YOUR FIRST DATA POINT HERE
    state_estimate_k_minus_1 = np.array([43.7812687642872333526611328125, -80.03338177688419818878173828125, 390.79998779296875])
    P_k_minus_1 = np.eye(3) * 0.1  # Identity matrix with 0.1 instead of 1

    # Time interval in seconds (you may want to adjust this)
    dk = 1

    # Initialize lists to store estimated latitudes and longitudes
    estimated_latitudes = []
    estimated_longitudes = []

    # Start at k=1 and go through each of the sensor observations
    for obs_vector_z_k in z_k:
        # Run the Extended Kalman Filter and store the near-optimal state and covariance estimates
        optimal_state_estimate_k, covariance_estimate_k = ekf(
            obs_vector_z_k,
            state_estimate_k_minus_1,
            P_k_minus_1
        )

        # Extract the estimated latitude and longitude
        estimated_latitudes.append(optimal_state_estimate_k[0])
        estimated_longitudes.append(optimal_state_estimate_k[1])

        # Get ready for the next timestep by updating the variable values
        state_estimate_k_minus_1 = optimal_state_estimate_k
        P_k_minus_1 = covariance_estimate_k

    plt.scatter(z_k[:, 1], z_k[:, 0], label='Actual Path', alpha=0.5)

    # Plot the estimated latitudes and longitudes as a scatter plot
    plt.scatter(estimated_longitudes, estimated_latitudes, label='Estimated Path')
    plt.xlabel('Longitude')
    plt.ylabel('Latitude')
    plt.title('Actual vs Estimated Path Using EKF')
    plt.legend()
    plt.show()

# Call the main function
main()