Kalman.ipynb

{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "colab": {
      "provenance": [],
      "authorship_tag": "ABX9TyPJvP2epRdkAVwEWjIGn1Y2",
      "include_colab_link": true
    },
    "kernelspec": {
      "name": "python3",
      "display_name": "Python 3"
    },
    "language_info": {
      "name": "python"
    }
  },
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "view-in-github",
        "colab_type": "text"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/Perfect-Cube/Smart-India-Hackathon-24/blob/main/Kalman.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "QcTMHoqDoKHI",
        "outputId": "2c02ac24-d094-4789-bd48-96eb8c5400be"
      },
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "Predicted: [[0. 0.]]\n",
            "Updated: [[0.67346939 1.34693878]]\n",
            "Predicted: [[1.04081633 2.08163265]]\n",
            "Updated: [[1.70998843 2.72232935]]\n",
            "Predicted: [[2.46278442 3.82607019]]\n",
            "Updated: [[2.82762168 3.94419051]]\n",
            "Predicted: [[3.76649884 5.1081773 ]]\n",
            "Updated: [[3.91844504 5.03778309]]\n",
            "Predicted: [[4.93073038 6.16776106]]\n",
            "Updated: [[4.97469826 6.06127719]]\n"
          ]
        }
      ],
      "source": [
        "import numpy as np\n",
        "\n",
        "class KalmanFilter:\n",
        "    def __init__(self, dt, u_x, u_y, std_acc, std_meas):\n",
        "        # Define sampling time\n",
        "        self.dt = dt\n",
        "\n",
        "        # Define the control input variables (external motion)\n",
        "        self.u = np.matrix([[u_x], [u_y]])\n",
        "\n",
        "        # Initial state vector [x_position, y_position, x_velocity, y_velocity]\n",
        "        self.x = np.matrix([[0], [0], [0], [0]])\n",
        "\n",
        "        # Define the state transition matrix A\n",
        "        self.A = np.matrix([[1, 0, self.dt, 0],\n",
        "                            [0, 1, 0, self.dt],\n",
        "                            [0, 0, 1, 0],\n",
        "                            [0, 0, 0, 1]])\n",
        "\n",
        "        # Define the control input matrix B\n",
        "        self.B = np.matrix([[(self.dt**2) / 2, 0],\n",
        "                            [0, (self.dt**2) / 2],\n",
        "                            [self.dt, 0],\n",
        "                            [0, self.dt]])\n",
        "\n",
        "        # Define measurement matrix H\n",
        "        self.H = np.matrix([[1, 0, 0, 0],\n",
        "                            [0, 1, 0, 0]])\n",
        "\n",
        "        # Define process noise covariance matrix Q\n",
        "        self.Q = np.matrix([[self.dt**4 / 4, 0, self.dt**3 / 2, 0],\n",
        "                            [0, self.dt**4 / 4, 0, self.dt**3 / 2],\n",
        "                            [self.dt**3 / 2, 0, self.dt**2, 0],\n",
        "                            [0, self.dt**3 / 2, 0, self.dt**2]]) * std_acc**2\n",
        "\n",
        "        # Define measurement noise covariance matrix R\n",
        "        self.R = np.matrix([[std_meas**2, 0],\n",
        "                            [0, std_meas**2]])\n",
        "\n",
        "        # Define the error covariance matrix P\n",
        "        self.P = np.eye(self.A.shape[1])\n",
        "\n",
        "    def predict(self):\n",
        "        # Predicted state estimate\n",
        "        self.x = np.dot(self.A, self.x) + np.dot(self.B, self.u)\n",
        "\n",
        "        # Predicted covariance estimate\n",
        "        self.P = np.dot(self.A, np.dot(self.P, self.A.T)) + self.Q\n",
        "\n",
        "        return self.x[0:2]\n",
        "\n",
        "    def update(self, z):\n",
        "        # Measurement residual\n",
        "        y = z - np.dot(self.H, self.x)\n",
        "\n",
        "        # Residual covariance\n",
        "        S = np.dot(self.H, np.dot(self.P, self.H.T)) + self.R\n",
        "\n",
        "        # Kalman gain\n",
        "        K = np.dot(self.P, np.dot(self.H.T, np.linalg.inv(S)))\n",
        "\n",
        "        # Updated state estimate\n",
        "        self.x = self.x + np.dot(K, y)\n",
        "\n",
        "        # Updated covariance estimate\n",
        "        I = np.eye(self.H.shape[1])\n",
        "        self.P = (I - np.dot(K, self.H)) * self.P\n",
        "\n",
        "        return self.x[0:2]\n",
        "\n",
        "# Example of using the KalmanFilter class for object tracking\n",
        "\n",
        "dt = 1.0  # Time step\n",
        "u_x, u_y = 0.0, 0.0  # Control input (acceleration)\n",
        "std_acc = 0.5  # Standard deviation of the acceleration (process noise)\n",
        "std_meas = 1.0  # Standard deviation of the measurement noise\n",
        "\n",
        "# Initialize the Kalman Filter\n",
        "kf = KalmanFilter(dt, u_x, u_y, std_acc, std_meas)\n",
        "\n",
        "# Simulated noisy measurements (positions)\n",
        "measurements = np.array([[1, 2], [2, 3], [3, 4], [4, 5], [5, 6]])\n",
        "\n",
        "for measurement in measurements:\n",
        "    predicted = kf.predict()\n",
        "    updated = kf.update(np.matrix(measurement).T)\n",
        "\n",
        "    print(f\"Predicted: {predicted.T}\")\n",
        "    print(f\"Updated: {updated.T}\")\n"
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "import numpy as np\n",
        "import folium\n",
        "\n",
        "# KalmanFilter class from previous code (paste here)\n",
        "\n",
        "# Generate some simulated noisy GPS data for a vehicle\n",
        "def generate_noisy_gps_data(true_path, noise_std):\n",
        "    noisy_path = []\n",
        "    for point in true_path:\n",
        "        noisy_point = point + np.random.normal(0, noise_std, point.shape)\n",
        "        noisy_path.append(noisy_point)\n",
        "    return np.array(noisy_path)\n",
        "\n",
        "# Initialize Kalman Filter and Map\n",
        "def map_vehicle_path(true_path, noisy_path, std_acc, std_meas):\n",
        "    # Initialize Kalman Filter\n",
        "    dt = 1.0\n",
        "    kf = KalmanFilter(dt, 0.0, 0.0, std_acc, std_meas)\n",
        "\n",
        "    # Create a folium map centered on the initial position\n",
        "    start_lat, start_lon = true_path[0]\n",
        "    vehicle_map = folium.Map(location=[start_lat, start_lon], zoom_start=15)\n",
        "\n",
        "    # Add the true path (for comparison)\n",
        "    folium.PolyLine(true_path, color='green', weight=2.5, opacity=0.8).add_to(vehicle_map)\n",
        "\n",
        "    # Add the noisy path (before Kalman filter)\n",
        "    folium.PolyLine(noisy_path, color='red', weight=2.5, opacity=0.6, dash_array='5').add_to(vehicle_map)\n",
        "\n",
        "    # Add optimized path after Kalman Filter\n",
        "    optimized_path = []\n",
        "\n",
        "    for i, measurement in enumerate(noisy_path):\n",
        "        kf.predict()\n",
        "        optimized_position = kf.update(np.matrix(measurement).T)\n",
        "        optimized_path.append(optimized_position.flatten().tolist()[0])\n",
        "\n",
        "    # Plot the optimized path\n",
        "    folium.PolyLine(optimized_path, color='blue', weight=2.5, opacity=0.8).add_to(vehicle_map)\n",
        "\n",
        "    # Save map to HTML file\n",
        "    vehicle_map.save('vehicle_path_map.html')\n",
        "    print(\"Map saved to 'vehicle_path_map.html'\")\n",
        "\n",
        "# Simulated true path of the vehicle (lat, lon)\n",
        "true_path = np.array([\n",
        "    [37.7749, -122.4194],  # San Francisco (start)\n",
        "    [37.7750, -122.4180],\n",
        "    [37.7755, -122.4170],\n",
        "    [37.7760, -122.4160],\n",
        "    [37.7765, -122.4150],  # (end point)\n",
        "])\n",
        "\n",
        "# Generate noisy GPS data\n",
        "noise_std = 0.001  # Standard deviation for GPS noise\n",
        "noisy_path = generate_noisy_gps_data(true_path, noise_std)\n",
        "\n",
        "# Use Kalman filter to smooth the noisy path and plot it\n",
        "map_vehicle_path(true_path, noisy_path, std_acc=0.1, std_meas=0.1)"
      ],
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "VUIZlLsGpeuw",
        "outputId": "ad095a38-74fa-4637-f8d2-9e10592ad3d4"
      },
      "execution_count": 3,
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "Map saved to 'vehicle_path_map.html'\n"
          ]
        }
      ]
    }
  ]
}