Introduction

Sensor fusion is the process of combining uncertain information to obtain an estimate that is more accurate than either source. Please watch the video below for a high level introduction to sensor fusion.

Kalman Filter

The Kalman filter is used to estimate the state of an object. Recall from Tuesday's lecture, the Kalman filter uses two steps: predict the next state and update the estimated state. In this lab, the state of the drone is a 2-dimensional vector containing the altitude and the vertical velocity. We combine information from three different sensors (the gps altitude, the pressure altitude, and velocity) to maintain an estimated altitude and vertical velocity of the drone.

Thus, the state vector has two components, the altitude and the vertical velocity:

Prediction Step

We start by defining the matrices and vectors used in the prediction step equations.

F is the state transition matrix. We assume a linear motion model with constant velocity.

u is the prediction noise. In our case, u is the zero vector, because we assume that the noise is a Gaussian distribution with mean zero.

The matrix P is the process covariance matrix.

The matrix P' is the posterior estimate of the process covariance matrix P.

Q is the covariance matrix of process noise. Since, in fact, the motion is not at constant velocity, but it has a vertical acceleration, the uncertainty caused by acceleration can be added to the noise component.

The kinematic equations for constant acceleration motion are:

The acceleration term is added to the noise:

The matrix Q is the covariance matrix of the normal noise distribution with two terms: a constant term which is the variance of normal distribution (the square of the standard deviation) of the acceleration and a term dependent on the time between measurements. The longer the time, the higher the uncertainty of the predicted position.

The state prediction equations are:

where x' is the predicted state or the posterior state, and x is the state and P' is the posterior estimate of the process covariance matrix.

In your code, x' and x can be represented by the same variable. The same is true for P' and P.

Update Step

The update step equations are the same for all measurements, but the constant matrices must be initialized with different values.

Matrices for the GPS and pressure measurements

For an GPS or pressure altitude measurement, the vector z has exactly one component. If the measurement is of the altitude type, then z is the measured altitude,

he H matrix projects the state vector x' onto the altitude component.

The matrix R is a 1x1 matrix, that is the measurement noise covariance matrix, and it is equal to the square of the standard deviation.

Matrices for the velocity measurements

For a vertical velocity measurement, the vector z has exactly one component.

The H matrix projects the state vector x' onto the vertical velocity component.

The matrix R is a 1x1 matrix, that is the measurement noise covariance matrix, and it is equal to the square of the standard deviation.

In the implementation, the attribute measurement_type of the measurement (an instance of the class Measurement) gives you the type of the measurement.

The measurement update equations are:

Implementing the Kalman Filter

Create a new node that uses the Kalman filter to estimate the altitude by fusing the information from three sensors: pressure altitude, gps altitude and velocity. You may use the code below as a starting point. The formulas for predict and update contain matrix operations. Use the numpy library functions for matrix addition, multiplication, transpose, inverse (https://numpy.org/).

The node is implemented in the class KalmanFilter. When a message is received on any of the three topics corresponding to the three sensors fused, an object of type Measurement containing the sensor type, the value and the time stamp is added to a queue. The mainloop of the KalmanFilter removes an object from the queue and the predict and update steps are executed. You will have to implement the predict and update steps, using the formulas described above.

You might notice that in the code below, we do not use locks anymore. We don't need to do it here, because the LifoQueue is thread-safe. All the complexities dealing with locks are encapsulated in the LifoQueue. Each subscriber adds a message to the queue on its own thread. The mainloop runs on another thread that keeps removing elements from the queue.

#!/usr/bin/env python

import rospy

from geometry_msgs.msg import TwistStamped

from altitude.msg import AltitudeStamped

import numpy as np

from Queue import LifoQueue

from enum import Enum 

class MeasurementType(Enum):

  PRESSURE_ALTITUDE = 1

  GPS_ALTITUDE = 2

  VELOCITY = 3

class Measurement:

    def __init__(self, measurement_type, value, timestamp):

        self.measurement_type = measurement_type

        self.value = value

        self.timestamp = timestamp

class KalmanFilter():

    def __init__(self):

        self.last_timestamp = None

        # the variance used in the matrix Q

        self.process_variance = rospy.get_param("/kalman_filter_node/process_variance", 100)

        # the variance used in the matrix R for the GPS measurements

        self.gps_variance = rospy.get_param("/kalman_filter_node/gps_variance", 0.1)

        # the variance used in the matrix R of the pressure altitude measurements

        self.pressure_variance = rospy.get_param("/kalman_filter_node/pressure_variance", 0.1)

        $ the variance used in the matrix R of the vertical velocity measurements

        self.velocity_variance = rospy.get_param("/kalman_filter_node/velocity_variance", 0.1)

        self.queue = LifoQueue()

        self.sub_pa = rospy.Subscriber('/uav/sensors/pressure_altitude',

                                      AltitudeStamped, self.process_pressure_altitude,

                                      queue_size = 1)

        self.sub_gps_a = rospy.Subscriber('/uav/sensors/gps_altitude',

                                      AltitudeStamped, self.process_gps_altitude,

                                      queue_size = 1)

        self.sub_vel = rospy.Subscriber('/uav/sensors/velocity',

                                      TwistStamped, self.process_velocity,

                                      queue_size = 1)

        self.pub =  rospy.Publisher('/uav/sensors/kalman_filter_altitude',

                                   AltitudeStamped,

                                   queue_size=1)

        self.mainloop()

    def process_pressure_altitude(self, msg):

        self.queue.put(Measurement(MeasurementType.PRESSURE_ALTITUDE,

                                      msg.value,

                                      msg.stamp))

    def process_gps_altitude(self, msg):

        self.queue.put(Measurement(MeasurementType.GPS_ALTITUDE,

                                      msg.value,

                                      msg.stamp))

    def process_velocity(self, msg):

        self.queue.put(Measurement(MeasurementType.VELOCITY,

                                      msg.twist.linear.z,

                                      msg.header.stamp))

    # The main loop of the node

    def mainloop(self):

        # Set the rate of this loop

        rate = rospy.Rate(20)

        # state vector

        x = np.zeros((2,1), dtype=float)

        # state transition matrix

        F = np.zeros((2,2), dtype=float)

        F[0][0] = 1

        F[1][1] = 1

        # process covariance matrix

        Q = np.zeros((2,2), dtype=float)

        # initialize P

        P = np.zeros((2,2), dtype=float)

        P[0][0] = 1

        P[1][1] = 1000

        # initialize I

        I = np.zeros((2,2), dtype=float)

        I[0][0] = 1

        I[1][1] = 1 

        # While ROS is still running

        while not rospy.is_shutdown():

            while not self.queue.empty():

                measurement = self.queue.get()

                if self.last_timestamp:

                    ########### predict step

                    delta_t = (measurement.timestamp - self.last_timestamp).to_sec()

                    # TODO: initialize matrices F and Q which depend on the delta_t

                    # TODO: update the state and the process matrix 

                    ########### update step

                    # TODO: initialize H for the measurement type 

                    # TODO: initialize R for the measurement type

                    # TODO: initialize the measurement z

                    z = np.zeros((1,1), dtype=float)

                    z[0] = #TODO

                    # TODO: implement the update equations 

                else:

                    # first measurement

                    if measurement.measurement_type == MeasurementType.VELOCITY:

                        x[1] = measurement.value

                    else:

                        x[0] = measurement.value

                # update timestamp

                self.last_timestamp = measurement.timestamp

                # publish message

                msg = AltitudeStamped()

                msg.value = x[0]

                msg.stamp = self.last_timestamp

                self.pub.publish(msg)

            # Sleep for the remainder of the loop

            rate.sleep()

if __name__ == '__main__':

    rospy.init_node('kalman_filter_node')

    try:

        KalmanFilter()

    except rospy.ROSInterruptException:

        pass

Checkpoint 2

Describe the Kalman filter when the state is a column vector with the following components: x, vx, y, vz, a, v, where x and y are the Cartesian coordinates and vx and vy are the velocities along those coordinates, a is the altitude (z coordinate) and v the corresponding velocity.

The gps sensor provides x,y, and altitude measurements. The velocity sensor provides measurements for vx, vy and v. The pressure altitude sensor provides altitude measurements.

For full credit, your answer must contain the sizes of the matrices and definition of the constant matrices.