Unscented Kalman filter implementation

from numpy.random import randn
import pyproj
import matplotlib.pyplot as plt
import numpy as np
import cv2
import os
import math
import xml.etree.ElementTree as ET
import pandas as pd
from filterpy.kalman import MerweScaledSigmaPoints
from filterpy.kalman import UnscentedKalmanFilter as UKF
from filterpy.common import Q_discrete_white_noise
from Functions import fx, hx

# Define the coordinate reference systems
src_crs = "EPSG:4326"  # WGS84
dst_crs = "EPSG:32632"  # UTM Zone 32N

# Create the coordinate transformation object
transformer = pyproj.Transformer.from_crs(src_crs, dst_crs, always_xy=True)

# Directories
filename = "Desktop/LABSF/Data/dataset.csv"
directory = "RGB/2022-07-15_08-58-03/"
radar_folder = "Desktop/LABSF/Data/Radar/XBR_imgs/"
xml_file = "Desktop/LABSF/annotations.xml"
image_directory = "Desktop/LABSF/Data/RGB/5min/"

# Timestamps interval
timestamp_1 = 1657876048.9269060
timestamp_2 = 1657876321.1003320

# Read the CSV file into a DataFrame
df = pd.read_csv(filename)

# Extract the directory column
image_column = df["rgb2"]
radar_column = df["image_data"]
xbr_max_column = df["xbr_max_range"]
latitude_column = df["latitude"]
longitude_column = df["longitude"]
heading_column = df["heading"]

# Create a list to store the filtered data
filtered_data = []

# Iterate over each directory path
for image, radar, xbr_max, lat, lon, heading in zip(image_column,radar_column,xbr_max_column,latitude_column,longitude_column,heading_column):
    # Extract the filename from the image directory path
    image_filename = os.path.basename(image)
    timestamp = float(os.path.splitext(image_filename)[0])

    # Check if the timestamp falls within the specified range
    if timestamp_1 <= timestamp <= timestamp_2:
        # Extract the filename from the radar directory path
        radar_filename = os.path.basename(radar)

        # Create the full paths for the image and radar files
        image_path = os.path.join(image_directory, image_filename)
        radar_path = os.path.join(radar_folder, radar_filename)

        # Check if the image and radar files exist
        if os.path.isfile(image_path) and os.path.isfile(radar_path):
            # Create dictionaries to store the data
            data = {
                "Image_path": image_path,
                "Radar_path": radar_path,
                "xbr_max_range": xbr_max,
                "Latitude": lat,
                "Longitude": lon,
                "Heading": heading,

            # Append the data to the filtered_data list

# Display the filtered data
for data in filtered_data:
    print("Image:", data["Image_path"])
    print("Radar:", data["Radar_path"])
    print("xbr_max_range:", data["xbr_max_range"])
    print("Latitude:", data["Latitude"])
    print("Longitude:", data["Longitude"])
    print("Heading:", data["Heading"])

# Create a list to store the boat positions in the NE frame
boat_positions = []

# Iterate over each data point
for data in filtered_data:
    # Extract the latitude, longitude, and heading from the data
    lat = data["Latitude"]
    lon = data["Longitude"]
    heading = data["Heading"]

    # Convert latitude and longitude to UTM coordinates
    utm_coords = transformer.transform(lon, lat)

    # Calculate the position in the NE frame
    # Assuming altitude is 0
    x = 0.0
    y = 0.0
    angle = math.radians(
        90 - heading
    )  # Convert heading to radians and convert to NE frame

    # Calculate the position in the NE frame
    x_ne = x + math.cos(angle) * utm_coords[0] - math.sin(angle) * utm_coords[1]
    y_ne = y + math.sin(angle) * utm_coords[0] + math.cos(angle) * utm_coords[1]

    # Create a dictionary to store the boat position
    boat_position = {
        "Latitude": lat,
        "Longitude": lon,
        "Heading": heading,
        "NE_X": x_ne,
        "NE_Y": y_ne,

    # Append the boat position to the list

# Display the boat positions in the NE frame
for position in boat_positions:
    print("Latitude:", position["Latitude"])
    print("Longitude:", position["Longitude"])
    print("Heading:", position["Heading"])
    print("NE_X:", position["NE_X"])
    print("NE_Y:", position["NE_Y"])

# Create lists to store the NE coordinates
ne_x_coords = []
ne_y_coords = []

# Iterate over each data point
for position in boat_positions:

# Plot the boat's movement in the NE frame
plt.plot(ne_x_coords, ne_y_coords, marker='o')
plt.title('Boat Movement in NE Frame')

# Read the XML file
tree = ET.parse(xml_file)
root = tree.getroot()

# Camera calibration
camera_yaw = 80
camera_yaw_rad = math.radians(camera_yaw)
camera_fov_w = 1.802212
camera_fov_h = 1.5708
image_width = 1920
image_height = 1080

# Calculate the horizontal and vertical center of the image
center_x = image_width / 2
center_y = image_height / 2

# Create empty dictionary to store image data
image_data = {}

for data in filtered_data:
    # Extract the image filename from the Image_path
    _, image_path_filename = os.path.split(data["Image_path"])
    # Find the corresponding image element in the XML file
    image_element = root.find(f".//image[@name='{image_path_filename}']")
    # Check if the image element exists
    if image_element is None:
        print("Image element not found for:", image_path_filename)
    # Get the filename of the image from the image element
    name = image_element.get("name")
    # Check if the image file exists
    if not os.path.isfile(data["Image_path"]):
        print("Image file does not exist:", data["Image_path"])

        # Load the image
        image = cv2.imread(data["Image_path"])
        # Check if the image was loaded successfully
        if image is None:
            print("Failed to load the image:", data["Image_path"])

            # Create empty list to store box data for the image
            box_data_image = []
            # Iterate over the box elements in the image element
            for box_element in image_element.iter("box"):
                # Get the label of the box
                box_label = box_element.get("label")

                # Extract the bounding box coordinates
                xmin = int(float(box_element.get("xtl")))
                ymin = int(float(box_element.get("ytl")))
                xmax = int(float(box_element.get("xbr")))
                ymax = int(float(box_element.get("ybr")))
                # Draw the bounding box on the image
                cv2.rectangle(image, (xmin, ymin), (xmax, ymax), (0, 255, 0), 2)
                # Display the label text near the bounding box
                label_text = f"{box_label}"
                label_text = f"{box_label.replace('Bouy', 'Buoy')}"
                    (xmin, ymin - 10),
                    (0, 255, 0),
                # Calculate the center of the box
                box_center_x = (xmin + xmax) / 2

                # Calculate the normalized coordinates (-1 to 1) based on the image size and center
                normalized_x = (box_center_x - center_x) / image_width * camera_fov_w
                # Transform the angle into the radar frame summing the yaw angle of rotation between the camera and the radar frame
                box_bearing = normalized_x - camera_yaw_rad

                # Store the box data in the box_data list
                # Add box data to the box_data_image list
                        "bearing": box_bearing,
                        "label": box_label,
            # Add the box_data_image to the image_data dictionary with image name as key
            image_data[name] = box_data_image
        # Display the image with the bounding boxes and annotations
        cv2.imshow('Image with Annotations', image)


for data in filtered_data:
    # Load the radar image
    radar_image_path = data["Radar_path"]
    radar_image = cv2.imread(radar_image_path)

    # Get the image name of the corresponding camera image
    image_name = os.path.basename(data["Image_path"])

    # Check if the image name exists in the image_data dictionary
    if image_name in image_data:
        # Get the box data associated with the image
        box_data = image_data[image_name]

        # Draw bearing lines and highlight points on the same side of the lines
        for box in box_data:
            bearing = box["bearing"]
            # Calculate the center point of the radar image
            center_x = radar_image.shape[1] // 2
            center_y = radar_image.shape[0] // 2

            # Calculate the line length based on the width of the radar image
            line_length = int(0.4 * radar_image.shape[1])

            # Calculate the endpoint coordinates of the line
            end_x = center_x + int(line_length * math.cos(bearing))
            end_y = center_y + int(line_length * math.sin(bearing))
            # Draw a line from the center to the endpoint
            cv2.line(radar_image, (center_x, center_y), (end_x, end_y), (0, 255, 0), 2)

            # Find the white spots in the radar image
            white_spots = np.where(radar_image[:, :, 2] > 200)

            # Calculate the distances between the white spots and the bearing line
            distances = np.abs(
                (end_y - center_y) * white_spots[1]
                - (end_x - center_x) * white_spots[0]
                + end_x * center_y
                - end_y * center_x
            ) / np.sqrt((end_y - center_y) ** 2 + (end_x - center_x) ** 2)

            # Threshold for considering a point on the bearing line
            threshold = 5  # Adjust as needed

            # Initialize variables to keep track of the last highlighted point
            last_highlighted_point = None
            highlighted_points = []
            # Highlight the points on the same side of the line
            for spot_y, spot_x, distance in zip(white_spots[0], white_spots[1], distances):
                dot_product = (spot_x - center_x) * (end_x - center_x) + (spot_y - center_y) * (end_y - center_y)
                if distance <= threshold and dot_product >= 0:
                    # Check if the point is different from the last highlighted point
                    if (spot_x, spot_y) != last_highlighted_point:
                        cv2.circle(radar_image, (spot_x, spot_y), 1, (0, 0, 255), -1)
                        highlighted_points.append((spot_x, spot_y))
                        last_highlighted_point = (spot_x, spot_y)

            # Extract the second highlighted point or the point following the last bearing
            if len(highlighted_points) >= 2:
                second_point = highlighted_points[1]
            elif len(highlighted_points) == 1:
                second_point = highlighted_points[0]
                # No highlighted points, set a default value
                second_point = None

            if second_point:
                # Extract max range for the radar image
                max_range = data["xbr_max_range"]

                # Calculate the distance of the second highlighted point
                x, y = second_point
                distance = (
                    * math.sqrt((x - center_x) ** 2 + (y - center_y) ** 2)
                    / line_length

                # Store the distance in the box data
                box.setdefault("distance", []).append(distance)

        # Update the box data in the image_data dictionary
        image_data[image_name] = box_data

        # Display the radar image
        cv2.imshow('Radar Image with Points on Bearing Line', radar_image)
        print("Image name not found in image_data:", image_name)


# Get the first image name from the image_data dictionary
first_image_name = next(iter(image_data.keys()))

# Get the box data for the first image
first_image_box_data = image_data[first_image_name]

# Initialize empty lists for bearings, labels, and distances
bearings = []
labels = []
distances = []

# Iterate over each box in the first image's box data
for box in first_image_box_data:
    # Check if the box has a 'bearing' and 'label' key
    if "bearing" in box and "label" in box:
        # Append the bearing and label to the respective lists

        # Check if the box has a 'distance' key
        if "distance" in box:
            # Append the distance to the distances list
            )  # Assuming there's only one distance value per box

# Create a dictionary with the extracted data for the first image
initial = {"bearing": bearings[0], "label": labels[0], "distance": distances[0]}

# Print the result dictionary

### UKF ###
# parameters
initial_bearing = bearings[0]  # First bearing
initial_distance = distances[0]  # First distance

# Convert bearing and distance to x and y coordinates
initial_x = initial_distance * np.cos(initial_bearing)
initial_y = initial_distance * np.sin(initial_bearing)

x = np.array([initial["bearing"], initial["distance"], 0, 0])  # initial state
P = np.diag([1, 1])  # initial uncertainty

points = MerweScaledSigmaPoints(n=2, alpha=1e-3, beta=2, kappa=0)
ukf = UKF(dim_x=2, dim_z=2, dt=1.5, fx=fx, hx=hx, points=points)

ukf.x = x  # initial state
ukf.P = P  # initial uncertainty
z_std1 = np.radians(5)  # degrees
z_std2 = 20  # meters
ukf.R = np.diag([z_std1**2, z_std2**2])  # measurement noise covariance matrix
ukf.Q = Q_discrete_white_noise(dim=2, var=0.01**2, dt=1.5, block_size=2)  # process noise covariance matrix

zs = [[data[1], data[3]] for data in image_data]
for i in range(1, len(boat_position)):
    z = zs[i]
    ukf.update(z, boat_position[i])  # Pass the boat's position for the i-th measurement
    print(ukf.x, 'log-likelihood', ukf.log_likelihood)```

I have some issue implementing the UKF, some ideas?

Please Tell us what’s happening in your own words.

Learning to describe problems is hard, but it is an important part of learning how to code.

Also, the more you say, the more we can help!

Sure, What I did is to take the information from the camera sensor and identify with bounding boxes the objects: Buoys or Boats. I calculated than their bearings referred to the camera. Furthermore I used the bearing to identify the objects into the radar images. However the distance obtained could not be accurate, as a matter of fact the radar doesn’t tell me what object I am looking at. In conclusion with all the measurements of bearings and distance I wanted to implement the UKF in my code, in order to estimate the objects and than compare the resoults with the measurements. The algorithm is not on-line, but based on a preorder set of frames.

Are you looking for some sort of help with this code?

1 Like

Yes, I am a bit lost in the end of it. I have no idea if my implementation is correct, and if there are other things that i need to add

are you able to help me? I have no rush, but if you need more info lmk

I have this code:

def fx(x, dt):
    # state transition functioin - predict next state based
    # on constant velocity model x = vt + x_0
    F = np.array([[1, 0, dt, 0],
                  [0, 1, 0, dt]])
    return np.dot(F,x)

def hx(x):
    # Extract the relative NE_X and NE_Y coordinates from the state vector x
    x, y = x[0], x[1]
    # Calculate distance
    distance = math.sqrt((x)**2 + (y)**2)

    # Calculate bearing in radians
    bearing = math.atan2(y, x)

    return np.array([[bearing], [distance]])

### UKF ###
### Preparation of the data ###
first_bearing = bearings[0]
first_distance = distances[0]

# Calculate the position of the object related to the boat frame
relative_ne_x = first_distance * math.cos(first_bearing)
relative_ne_y = first_distance * math.sin(first_bearing)

### Define the initial values and the functions ###

# Store the values in variable x if needed
x = np.array([relative_ne_x, relative_ne_y]) # Initial state
P = np.diag([1, 1])  # initial uncertainty

However, I get this error, even if everything seems correct:

Traceback (most recent call last):
  File "Desktop/LABSF/Main.py", line 354, in <module>
  File "Desktop/LABSF/env/lib/python3.9/site-packages/filterpy/kalman/UKF.py", line 388, in predict
    self.compute_process_sigmas(dt, fx, **fx_args)
  File "Desktop/LABSF/env/lib/python3.9/site-packages/filterpy/kalman/UKF.py", line 503, in compute_process_sigmas
    self.sigmas_f[i] = fx(s, dt, **fx_args)
TypeError: fx() takes 1 positional argument but 2 were given

I tried to modify all the code increasing the number of initial states with the x and y velocities, but the problem remains, instead of having 1 and passing 2 I will have 2 and passing 4.