UAV Drone: Object Tracking using Kalman Filter

Problem Statement

In our scenario, we have a drone that is engaged in delivering packages within a bustling urban environment. Unlike other existing systems in the market where packages are dropped from above using parachutes, our innovative approach employs a cord mechanism. This cord is securely attached to the package and is gradually lowered once the drone reaches its destination. While this novel system offers an enhanced user experience and prioritizes the well-being of all involved parties, it does come with a potential drawback. There is a possibility that an individual or an animal, either intentionally or unintentionally, may pull or trip on the attached cord. Such an action poses risks to both the user and the drone, potentially leading to injuries in certain circumstances.

The drone is equipped with an RGB camera and a Stereo camera. Utilizing the data collected by these cameras, we can develop a sophisticated tracking system. This system will enable the drone to safely lower the package to the ground, ensuring that it does so only when it predicts a minimal risk of the cord being pulled. By analyzing the camera data, the tracking system will be capable of identifying and tracking individuals or animals that are in close proximity to the intended landing position of the drone's package. This proactive approach will allow the drone to avoid potential risks and ensure the safe delivery of packages without endangering users or causing harm to surrounding individuals or animals.

While computer vision-based tracking algorithms have been widely used, it's important to consider the challenges posed by urban delivery environments. In city settings, the presence of obstructions such as trees, electric poles, and buildings can hinder the effectiveness of computer vision alone. Additionally, the dynamic nature of urban environments introduces a multitude of moving objects that need to be accounted for. To ensure safe and reliable operations, it is crucial for our tracking system to be capable of accurately detecting and tracking not only stationary objects but also individuals or animals approaching the drone's landing position, as they pose a significant risk of collision with the drone or the package it carries.

In the video below, an obstructed object is shown where YOLOv8 fails to detect it. However, utilizing the Kalman Filter, we are still able to predict the future states of the object by leveraging the Process model and the current states.

kf_obs_github.mp4

Abstract

This project involved designing and implementing a linear Kalman filter from scratch to track both stationary objects and moving entities, such as individuals or animals, near the drone's landing position. Real-world testing demonstrated the filter's effectiveness in providing accurate position and velocity estimates, reducing the risk of collision. However, limitations were observed in handling non-linear dynamics and uncertainties. It was concluded that non-linear Kalman filters, like the Extended Kalman Filter or Unscented Kalman Filter, would be more suitable for complex scenarios. Further research on non-linear Kalman filters is recommended to improve tracking accuracy and robustness in dynamic environments.

Brian Douglas's blog, Bzarg's online tutorial, and Dr. Steven Dumble's course provided essential guidance and insights on Kalman filters and tracking algorithms. Their resources greatly influenced my understanding and implementation of the Kalman filter, enhancing the success of this project. I sincerely appreciate their valuable contributions and acknowledge their significant impact on my work. Some of the graphs and visualization have been adapted from their work.

---

Plan of Action

1. Basics of Kalman Filter

2. Kalman Filter Implementation

3. Autonomous Car Tracking

4. UAV Object Tracking

---

1. Basics of Kalman Filter

1.1 The Origin

The Kalman Filter was invented by the great Rudolf E. Kálmán who received the National Medal of Science on Oct. 7, 2009, from President Barack Obama at the White House. Kalman filters were first used during the Apollo space program that put men on the moon, in the NASA Space Shuttle, U.S. Navy submarines, and in unmanned aerospace vehicles and weapons.

Image Source: Obama honors former UF professor with science medal

1.2 The Purpose

Let's see examples of why we may need Kalman Filters.

Example 1: We need to measure the internal temperature of a core reactor of a nuclear power plant. You may guess that we cannot put any type of sensors directly into the core as they will melt instantaneously. The best we can do is measure external temperature outside the core and then use a Kalman Filter to find the best estimate of the internal temperature from an indirect measurement. That is, we use a Kalman Filter when we cannot measure directly the variables of interest.

Example 2: Suppose we want to track the location of our drone at a specific time t. We may use the a GPS to do so but the precision may differ based on the number of satellites available and other factors. We may use the onboard IMU sensor to deduce the distance traveled but our sensor can be noisy. So what we can do is fuse both sensors' measurements to find the optimal estimate of the exact location of our drone. That is, we use Kalman Filter when we want to find the best estimate of states by combining measurements from various sensors in the presence of noise.

To put it simply, our aim is to determine the current state of the system by considering both its underlying dynamics and the presence of imperfect measurements.

1.3 Types of Kalman Filter

The choice of the filter depends on the specific characteristics of the system being modeled and the accuracy requirements of the application. In some cases, it may be necessary to perform experiments or simulations to determine which filter performs best for a given situation.

Linear Kalman Filter

• Assumes the system dynamics and measurement models are linear.
• Updates the state estimate and covariance directly using linear equations.
• Suitable for systems that can be accurately represented by linear models.
• Widely used in applications such as tracking, navigation, and control systems.

Extenden Kalman Filter

• Extends the linear Kalman filter to handle nonlinear system dynamics and/or measurement models.
• Approximates the nonlinear models using a first-order Taylor series expansion.
• Requires calculating Jacobian matrices for nonlinear models.
• Used when the system dynamics or measurements exhibit nonlinear behavior.
• Commonly used in applications such as robotics, autonomous vehicles, and aerospace systems.

Unscented Kalman Filter

• Overcomes the need for linearization in the EKF by using a deterministic sampling-based approach.
• Represents the probability distribution of the state using a set of sigma points.
• Propagates these sigma points through the nonlinear functions to estimate the mean and covariance of the state.
• More accurate than the EKF for highly nonlinear systems.
• Suitable for applications where the system dynamics are highly nonlinear and/or the EKF fails to provide accurate results due to significant nonlinearities in the system.

1.4 The States

Kalman Filter (KF) are ideal for systems that are continuously changing. Why? Because KF keeps only the previous state hence, they are fast and well-suited for real-time problems.

So what is this "state" term? The state is the underlying configuration of our system. It can be the position, velocity, volume, temperature and so on of our system. We will take the example of our drone which has two states: position and velocity. And it will be represented in a state vector.

Now that we know what are states, let's define the steps of a KF. In a nutshell, KF can be expressed in a two-step process: prediction and update which provides an optimal state estimate.

Suppose we want to track the position of our drone at time t. Our GPS and other sensors have noise hence, will not provide an accurate result. But what if we can provide a mathematical model of our system? With Newton's Equation of Motion, we can predict at time t+1 what will be the position of our drone using its velocity. However, this will be a flawed model as we are not taking into account external factors such as wind resistance, snow, rain, and so on. Hence, this is a flawed model. In summary, we have a sensor with uncertainty and a mathematical model with its own uncertainty. What if we could combine both? And that is exactly what the Kalman Filter does.

• The Kalman Filter will combine measurements from a noisy sensor and the prediction of a flawed model to get a more accurate estimate of the system state than either one independently.

• The Kalman Filter computes the ideal weightage for the measured and predicted states, so that the resulting corrected state is positioned precisely at the optimal location between them.

Note that we do not know what is the actual position and velocity due to the uncertainties. But we do know that there is a whole range of possible combinations that may be true.

One important assumption of KF is that it expects our variables to be random and Gaussian distributed. Hence, if we denote our variables using a state vector x. Each variable has a mean and a variance such that:

Note that the variables may be correlated. For example, here, position and velocity are correlated as a higher velocity expects to give a higher position. Hence, this correlation is captured in a covariance matrix:

A positive covariance indicates that the variables tend to increase or decrease together, while a negative covariance indicates that as one variable increases, the other tends to decrease.

• The Kalman filter works by propagating the mean and covariance of the Gaussian distributions for the estimated state through time using a linear process model. This is known as the prediction step or process update, where the estimates move forward in time.

• When measurements are available, which are a linear combination of the state, the Kalman filter updates the estimated state's mean and covariance based on the measurements' mean and covariance of the Gaussian distribution. This is the update step or corrections step for the Kalman filter.

• These two processes form the prediction and correction step, which is recursively run in the filter with time. The estimates are propagated forward in time to the current time. If the measurement information is available at the current time, it is used to improve the current state estimates. This is then repeated as time moves forward,

---

1.5 Process Model: Prediction

We will denote our current state as k-1 and the next state we will predict as k. Our best estimate at the current time step k-1 will be:

And the next best estimate at time step k will be:

In we expand our state vector to represent a 2D model:

Again, we do not know which state is the actual one, even the initial state has uncertainty, hence, our prediction model will output a new distribution after working on all the possible values.

Let's try to predict the position and velocity at the next time step. Assuming a constant velocity (zero acceleration) scenario, therefore:

We predict the position as such:

The two last equations can be written in matric form as such:

In 2D:

Re-writting again with being the State Transition Matrix.:

The State Transition Matrix propagates the state at sample k-1to the state sample k.

Now we need to propagate the error covariance forward. Note that uncertainty comes from 2 sources when predicting:

1. Initial uncertainty in the system before prediction
2. Additional uncertainty as we propagate the state forward in time

We will define the initial uncertainty first with known as the Prediction Error Covariance Matrix:

Note that we use the State Transition Matrix F to propagate the initial error covariance.

1.6 Process Model: External Influence

In the first part of our prediction model, we assumed a constant velocity. But that is not always the case in the real world. External forces may cause a system to accelerate. There may be changes that are not related to the state itself. Thus, we assume a linear acceleration.

We use Newton's Equation of Motion to update our model:

Re-writing it in matrix form:

In 2D:

is the control matrix and is called the control vector. By factoring in the system's dynamics and the effects of external controls on its future state, we can derive an estimated state projection

However, when we do external tracking we may not know about this input vector. Instead of having a known deterministic input, we assume that the input is just going to be due to noise. Hence, we replace with which is the Process Model Noise Sensiivity Matrix and the Process Model Noise vector respectively.

We then have:

1.7 Process Model: External Uncertainty

It is impossible to model every external force (friction, air resistance,...) acting on our system. In order to counter that, we add some new uncertainty after every prediction step. We will model the second source of uncertainty into our system: uncertainty as we propagate the state forward in time.

• Each blue point in the diagram above can be our current state though it is most likely the actual state will be at the center of the Gaussian blob.
• When predicting the next time step, a new Gaussian blob is created whereby each new green point can be the actual predicted state.
• Each predicted point (green) has its own Gaussian blob, i.e., when we deal with unobserved factors that can impact the system's performance, we treat them as random disturbances or noise with a particular covariance matrix.

The new Prediction Error Covariance Matrix is:

where:

where

and

Note that we build up the new uncertainty from the old one by adding some uncertainty from the environment. This new uncertainty is proportional to the time horizon. That is, the further we predict, the bigger this new uncertainty will be. So, the Prediction Covariance Matrix grows over time.

This additional uncertainty is modelled by the Process Noise Covariance Matrix, . This matrix captures all uncertainty which we cannot model coming from unknown inputs or from the discrepancy in our predicting model.

To sum up:

1. The new best estimate is a prediction model from the previous best estimate + a correction for known external influences.
2. The new uncertainty is predicted from the old uncertainty + additional uncertainty from the environment.

---

1.8 Measurement: Noise & Estimate

Recall that the Kalman Filter will combine measurements from a noisy sensor and the prediction of a flawed model to get a more accurate estimate of the system state than either one independently. What we have done since then is build a mathematical model in order to predict the next state of our system. What we will do now, is take readings from our sensor in order to get a measurement estimate.

When we buy a sensor, the manufacturer tells us about the precision of the module. Precision is how close all the sensor readings are grouped to each other. When we need to measure distance using a laser for example, we cannot expect to get the same reading every time we take a measurement with our sensor. There will be some small variations and this is due to random noise in the sensor.

One important factor is that the scale of readings from our sensor may not be of the same scale as the states we are measuring. Hence, we’ll model the sensors with a matrix, . We can use this matrix to formulate a distribution of what our predicted measurement would be using the next best estimate + its uncertainty of our prediction model:

Note that P is rotated into the sensor frame using H. As we analyze a reading, we can make an educated guess about the state of our system. However, due to the presence of uncertainty, some states have a greater probability of producing the observed reading than others.

• We denote the Measurement Covariance Matrix as , which reflects the inherent variability and uncertainty in our sensor measurements.
• This new distribution from our sensor readings has a mean represented with a vector:

From the diagram above, notice how as we propagate further into the future our Process Noise Covariance Matrix, , grows bigger and bigger. In total, we have 3 error covariance matrices: , , .

To sum up, we now have 2 Gaussian blobs:

1. One surrounding the mean of our transformed prediction (green)
2. One surrounding the actual sensor reading we got (pink)

• Our next step will be to reconcile our guess about the readings we would see based on the predicted state with a different guess based on our sensor readings that we actually observe.
• To find our new most likely state, we simply need to multiply the two Gaussian distributions and the output will be another Gaussian distribution where the mean of this distribution is the configuration for which both estimates are most likely, and is, therefore, the best guess of the true configuration given all the information we have.

---

1.9 Gaussian Multiplication

As explained above, if we multiply two Gaussian distributions, our output will still be a Gaussian distribution. We will have something like this:

After some simplification, our new mean can be expressed as:

and the new variance as:

We can factor out k where k:

Hence, re-writing the new mean and new covariance:

Re-writing them in matrix form:

Note that K is the Kalman Gain which is the ratio of the covariance of the predicted state estimate to the sum of the covariance of the predicted state estimate and the covariance of the measurement.

The Kalman gain ratio plays a crucial role in determining the weight given to the current measurement and the predicted state estimate during the update step of the Kalman filter. It represents the relative importance of the measurement in adjusting the state estimate.

• A higher Kalman gain ratio means that the measurement has a larger influence on the updated state estimate.
• A lower ratio indicates a greater reliance on the predicted state estimate.

---

2.0 Putting all together

We have 2 Gaussian distributions: one for the process model and another for the measurements:

The new mean for our new Gaussian distribution is then:

And the new covariance is:

The Kalman Gain is then defined as:

Let's define some more terms.

Innovation Residual

The innovation residual represents the discrepancy between the predicted measurement and the actual measurement.

Innovation Covariance

The innovation covariance represents the measure of uncertainty in the prediction error between the estimated measurement and the actual measurement.

Kalman Gain

The Kalman gain determines the optimal weight given to the predicted state and the measurement to obtain an improved state estimate.

Corrected State Estimate

The corrected state estimate is the updated estimation of the true state, obtained by combining the predicted state estimate with the measurement using the Kalman gain.

Corrected Estimate Covariance

The corrected estimate covariance represents the updated measure of uncertainty in the estimated state after incorporating the measurement information. It reflects the accuracy of the corrected state estimate.

Note that is less than 1 hence, is always smaller than . This way we reduce uncertainty in the estimates and improve accuracy.

2. Kalman Filter Implementation

We will now code our Kalman Filter from scratch. We start with a Kalman filter class and define the following parameters in the __ init __ function:

class KalmanFilter(object):
    def __init__(self, dt, INIT_POS_STD, INIT_VEL_STD, ACCEL_STD, GPS_POS_STD):
        
        """
        :param dt: sampling time (time for 1 cycle)
        :param INIT_POS_STD: initial position standard deviation in x-direction
        :param INIT_VEL_STD: initial position standard deviation in y-direction
        :param ACCEL_STD: process noise magnitude
        :param GPS_POS_STD: standard deviation of the measurement
        """

We do not know our initial state hence, we declare it as zero values:

        # Intial State
        self.x = np.zeros((4, 1))

We want to initialize our covariance matrix based on the standard deviation of the position and velocity as such:

        # State Estimate Covariance Matrix
        cov = np.zeros((4, 4))
        cov[0, 0] = INIT_POS_STD ** 2
        cov[1, 1] = INIT_POS_STD ** 2
        cov[2, 2] = INIT_VEL_STD ** 2
        cov[3, 3] = INIT_VEL_STD ** 2
        self.P = cov

The state transition matrix:

        # State Transition Matrix
        self.F = np.array([[1, 0, self.dt, 0],
                           [0, 1, 0, self.dt],
                           [0, 0, 1, 0],
                           [0, 0, 0, 1]])

Next we define the process model noise using the covariance of the process model noise and the process model noise sensitivity matrix:

        # Covariance of Process model noise
        q = np.zeros((2, 2))
        q[0, 0] = ACCEL_STD ** 2
        q[1, 1] = ACCEL_STD ** 2
        self.q = q

        # Process Model Sensitivity Matrix
        L = np.zeros((4, 2))
        L[0, 0] = 0.5 * self.dt ** 2
        L[1, 1] = 0.5 * self.dt ** 2
        L[2, 0] = self.dt
        L[3, 1] = self.dt
        self.L = L

        # Process model noise
        self.Q = np.dot(self.L, np.dot(self.q, (self.L).T))

Lastly, we want to define our Measurement matrix and the Measurement covariance matrix:

        # Define Measurement Mapping Matrix
        self.H = np.array([[1, 0, 0, 0],
                           [0, 1, 0, 0]])

        # Measurement Covariance Matrix
        R = np.zeros((2, 2))
        R[0, 0] = GPS_POS_STD ** 2
        R[1, 1] = GPS_POS_STD ** 2
        self.R = R

The predict() function projects the current state estimate and error covariance forward to the next time step. It calculates the predicted state estimate and the predicted error covariance using the state transition matrix F and the process noise covariance matrix Q. This step is crucial for updating the state estimate based on the system dynamics and accounting for the uncertainty introduced by the process noise.

    # PREDICTION STEP
    def predict(self):
        self.x = np.dot(self.F, self.x)
        self.P = np.dot(self.F, np.dot(self.P, (self.F).T)) + self.Q
        return self.x

In the update function, we calculate the Kalman gain and use it to update the predicted state estimate and predicted error covariance . This step involves incorporating the measurement information and adjusting the state estimate based on the measurement residuals and the measurement noise covariance matrix R. By applying the Kalman gain, we obtain an improved estimate of the true state, taking into account both the predicted state and the available measurement information.

    # UPDATE STEP
    def update(self, z):
        # Innovation
        z_hat = np.dot(self.H, self.x)
        self.y = z - z_hat

        # Innovation Covariance
        self.S = np.dot(self.H, np.dot(self.P, (self.H).T)) + self.R

        # Kalman Gain
        self.K = np.dot(self.P, np.dot((self.H).T, np.linalg.inv(self.S)))

        I = np.eye(4)

        self.x = self.x + np.dot(self.K, self.y)
        self.P = np.dot((I - np.dot(self.K, self.H)), self.P)
        return self.x

---

3. Autonomous Vehicle Tracking

Now we need to test if our implementation really works. Consider an autonomous vehicle starting at position (0,0)traveling at a constant speed of 5 m/s with a heading of 45 degrees. We have a GPS tracking the x and y positions of the vehicle.

# Car parameters
initial_position = np.array([0, 0])
speed = 5.0  # m/s
heading = 45.0  # degrees

# Convert heading to radians
heading_rad = math.radians(heading)

We calculate the true positions based on constant speed and heading and use a Gaussian distribution to generate the measurements.

# Calculate true positions based on constant speed and heading
true_positions = [initial_position]
for _ in range(num_measurements-1):
    delta_x = speed * math.cos(heading_rad)
    delta_y = speed * math.sin(heading_rad)
    new_position = true_positions[-1] + np.array([delta_x, delta_y])
    true_positions.append(new_position)
true_positions = np.array(true_positions).T

# Add noise to simulate measurement errors
measurement_noise_std = 5
measurements = true_positions + np.random.normal(0, measurement_noise_std, size=true_positions.shape)

We define the Kalman Filter parameters as such:

# Kalman filter parameters
dt = 1.0  # Sampling time
INIT_POS_STD = 10  # Initial position standard deviation
INIT_VEL_STD = 10  # Initial velocity standard deviation
ACCEL_STD = 5  # Acceleration standard deviation
GPS_POS_STD = 3  # Measurement position standard deviation

We then apply the Kalman filter to calculate the estimated states:

# Kalman filter initialization
kf = KalmanFilter(dt, INIT_POS_STD, INIT_VEL_STD, ACCEL_STD, GPS_POS_STD)

# Lists to store filtered states
filtered_states = []

# Perform prediction and update steps for each measurement
for measurement in measurements.T:
    # Prediction step
    predicted_state = kf.predict()

    # Update step
    updated_state = kf.update(measurement)

    # Store the filtered state
    filtered_states.append(updated_state)

# Convert filtered states to NumPy array
filtered_states = np.array(filtered_states).T

We then plot our filtered or estimated states along with the true position and the measurements. We observe that initially, our estimated states follow the GPS measurements. But with time, it converges close to the true value even when the GPS measurements are off by a lot as indicated by the yellow arrows.

Now, we will test our filter using the simulation created by Dr. Steven Dumble in his online course. It is using the same Kalman Filter we designed above but here we used C++.

1. ACCEL_STD = 0

• The position and velocity uncertainty starts to the true value.
• The estimates are going to use the information contained inside the process model and converge to the truth.
• However, this won't work if we have a more dynamic system where the car changes direction abruptly.
• By having zero process model noise, the filter is going to become inconsistent because the estimated position is not close to the true position.

ACCEL_STD.1.0._.GPS_POS_STD.3.-.Made.with.Clipchamp.3.mp4

2. ACCEL_STD = 0.1

• As prediction happens, the uncertainty inside the system is inflating.
• When the car changes direction, the estimated state catches up with the true state a lot quicker than it did when we had zero process model uncertainty.
• Notice that now, we have lots of jumping around with the estimates because the amount of uncertainty in the system is growing quite rapidly.
• That is, the estimates are going to rely on the GPS measurements more than the process model.

ACCEL_STD.1.0._.GPS_POS_STD.3.-.Made.with.Clipchamp.2.mp4

3. ACCEL_STD = 1.0

• We have a higher ACCEL_STD, and we have a larger uncertainty growth with time.
• The estimate catches up quicker with the true state in the corners.
• The state estimates follow the GPS measurements a lot more. It is going to trust them a lot more because it got less uncertainty inside the prediction model.

ACCEL_STD.1.0._.GPS_POS_STD.3.-.Made.with.Clipchamp.1.mp4

In summary:

1. GPS_std directly affects the size of R, however Acc_std affects the Rate Of Increase in P (not directly the size of P). If Acc_std is large, then P will grow quickly, so that when a new GPS measurement is made, the value of P will be greater than R. Therefore, the system will trust the GPS more, since R is smaller than P.

2. If we want a nice smooth response that trusts the prediction model more, we want a lower noise value for the acceleration. (Lower Q)

3. However, if the car changes direction a lot and we want the process model to try to keep up, we are going to need a larger acceleration standard deviation to let the filter update quickly for the new response. (Higher Q)

4. We assumed the initial state to be zero however, we would also have waited for the first measurement. Either way, the Linear Kalman Filter will always be updated to minimize the estimation error.

---

4. UAV Object Tracking

Kalman filters are used to track the positions and velocities of objects of interest, such as other vehicles, pedestrians, or stationary obstacles. By fusing data from different sensors, such as cameras, the filter estimates the object's state with high accuracy, even in the presence of sensor noise or measurement uncertainties. This tracking information is crucial for collision avoidance as it allows the drone to perceive the current and future positions of objects in its environment.

In this scenario, we do not want to track the drone but instead people or animals approaching the drone's landing position. This is important because these moving objects can potentially collide with the drone or the package it is carrying as shown below.

Using YOLOv8, we will get the bounding box of the region of interest and consecutively its center. We will use the centers as GPS Measurements that we had in the simulation before. We will still use the same Process Model we designed and update our estimates based on the center of the bounding boxes.

  # Process the frame to get bounding box centers
    centers = get_bounding_box_center_frame(frame, model, names, object_class='person')

With a low value for ACCEL_STD, the estimates were not as close to the real value. I had to increase it to 40 such that the estimates now rely on the center of the bounding boxes more than the process model.

We check first if there is a bounding box, if there is we perform the predict function of our Kalman Filter then we perform the update function using the center of the bounding box. We plot the value for the predict, update and the center to check how the model is performing.

    # Check if center is detected
    if len(centers) > 0:
        center = centers[0]  # Extract the first center tuple

        # Draw circle at the center
        if isinstance(center, tuple):
            print("Center = ", center)
            cv2.circle(frame, center, radius=8, color=(0, 255, 0), thickness=4) # Green

            # Predict
            x_pred, y_pred = kf.predict()
            if isFirstFrame:  # First frame
                x_pred = round(x_pred[0])
                y_pred = round(y_pred[0])
                print("Predicted: ", (x_pred, y_pred))
                isFirstFrame = False
            else:
                x_pred = round(x_pred[0])
                y_pred = round(y_pred[1])
                print("Predicted: ", (x_pred, y_pred))

            cv2.circle(frame, (x_pred, y_pred), radius=8, color=(255, 0, 0), thickness=4) #  Blue

            # Update
            (x1, y1) = kf.update(center)
            x_updt = round(x1[0])
            y_updt =  round(x1[1])
            print("Update: ", (x_updt, y_updt))
            cv2.circle(frame, (x_updt, y_updt), radius=8, color= (0, 0, 255), thickness=4) # Red

Below are two examples where the object changes direction abruptly. This is a good example to see how our Linear Kalman Filter is performing on a dynamic non-linear system. We observe that there is still a discrepancy between the true value and the estimated one. This is because of the process model we are using which is a linear model.

running-3-kf_k4MGU7yG.mp4

By increasing the ACCEL_STD to a high value, we rely much more on the detector than our process model. This is working fine for now however, if the detector wrongly detects the position of the object then our estimates will be off from the true value as well.

running-4-kf_Z9Oiz2iG.mp4

Conclusion

In conclusion, the implementation of a linear Kalman filter in our project has proven to be instrumental in achieving accurate and reliable tracking of both stationary objects and moving entities, such as individuals or animals, near the drone's landing position. By continuously estimating their position, the Kalman filter enables us to anticipate and respond to potential collision risks effectively. This capability is crucial in urban environments where obstructions and dynamic elements pose challenges to the drone's path. With the Kalman filter, we can enhance the safety and reliability of the drone operations, mitigating the risks associated with collisions and ensuring the successful delivery of packages. By combining computer vision with the Kalman filter, we have developed a robust tracking system that contributes to the overall efficiency and effectiveness of our delivery services.

In addition, while the linear Kalman filter has provided satisfactory results, it is worth noting that more advanced tracking algorithms such as DeepSORT or ByteTrack may offer enhanced performance and robustness in scenarios with complex dynamics and uncertainties. These algorithms leverage deep learning techniques and non-linear models to handle more challenging tracking scenarios effectively. In future iterations of our project, integrating these advanced tracking algorithms could further improve our ability to track and anticipate the movements of objects and individuals, ensuring even greater safety and efficiency in drone deliveries.

References

[1] Engineering Media. (n.d.). The Kalman Filter. Webpage. https://engineeringmedia.com/controlblog/the-kalman-filter

[2] Bzarg, C. (n.d.). How a Kalman Filter Works in Pictures. Webpage. https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

[3] MATLAB. (n.d.). Kalman Filters and Control. YouTube playlist. https://www.youtube.com/watch?v=mwn8xhgNpFY&list=PLn8PRpmsu08pzi6EMiYnR-076Mh-q3tWr&ab_channel=MATLAB

[4] MathWorks. (n.d.). Understanding Kalman Filters. Video series. https://www.mathworks.com/videos/series/understanding-kalman-filters.html

[5] Arshren. (n.d.). An Easy Explanation of Kalman Filter. Webpage. https://arshren.medium.com/an-easy-explanation-of-kalman-filter-ec2ccb759c46

[6] Udemy. (n.d.). Advanced Kalman Filtering and Sensor Fusion. Course. https://www.udemy.com/course/advanced-kalman-filtering-and-sensor-fusion/

[7] Michel van Biezen. (n.d.). Kalman Filter Lectures. YouTube playlist. https://www.youtube.com/watch?v=CaCcOwJPytQ&list=PLX2gX-ftPVXU3oUFNATxGXY90AULiqnWT&ab_channel=MichelvanBiezen

[8] Kalman Filter. (n.d.). Website. https://www.kalmanfilter.net/default.aspx

[9] Zucconi, A. (2022, July 24). Kalman Filter [Part 3]. Webpage. https://www.alanzucconi.com/2022/07/24/kalman-filter-3/

[10] Machine Learning Space. (n.d.). 2D Object Tracking using Kalman Filter. Webpage. https://machinelearningspace.com/2d-object-tracking-using-kalman-filter/

[11] Towards Data Science. (n.d.). Kalman Filter Interview. Webpage. https://towardsdatascience.com/kalman-filter-interview-bdc39f3e6cf3