kalman_filter_v2 module

class kalman_filter_v2.KalmanFilter(debug=False)

Bases: object

Class for Extended Kalman Filter

States and Control Inputs are defined as :- State X = [x, y, z, x’, y’, z’] Control Input U = [Upward Thurst (T - mg), Pitch Torque, Roll Torque, Yaw Torque]

Uses Quadrotor Dynamics to perform Extended Kalman Filtering Estimation using Aruco Position Data and IMU’s Acceleration Data.

apply_system_dynamics(imuData, U, dt)

Part of Prediction Step that predicts states, and is used with IMU Data and Control input to get a better estimate of states.

Parameters:

imuData: IMU Data
U: Throttle (As Required by Quadrotor Dynamics)
dt: Time Elapsed since last update

estimate_pose(control_inp, sensor_obs, dt)

Uses Aruco Position and IMU Data, and fuses the data using Extended Kalman Filter to given a better estimate of the states.

Parameters:

control_inp: Control Input provided to the drone
sensor_obs: Readings from Aruco and IMU
dt: Time Elapsed since last update

Returns:

X: New estimated states after applying EKF

update_measurement(H, sensor_obs, sensor_noise_bias, sensor_noise_cov)

Performs the main step of Extended Kalman filtering and updates the state prediction to better estimate the states.

Parameters:

H: Observation Matrix
sensor_obs: Sensor Reading
sensor_noise_bias: Sensor Bias
sensor_noise_cov: Sensor Noise Covariance

kalman_filter_v2.get_angle_rate(imuData)

The function takes in Gyro Data from IMU Data and gives euler angular rates which is obtained after transformation.

Parameters:

imuData: Current Readings of IMU Data

Returns:

A list containing
roll_rate: Rate of roll
pitch_rate: Rate of pitch
yaw_rate: Rate of yaw

kalman_filter_v2.get_pos(prevPos, currentVelocity, dt)

The function takes in previous position in world frame and current velocity to return current position, by using Euler Forward Method.

Parameters:

prevPos: Previous timestep position in the world frame
currentVelocity: Current velocity in the world frame

Returns:

Current Position

kalman_filter_v2.get_velocity(prevVelocity, CurrentAcc, phi, theta, psi, dt)

Returns the current velocity. The previous timestep velocity is taken with respect to world frame and transformed to body frame, where it is updated using acceleration data of the accelerometer and then again transformed to the world frame.

Parameters:

prevVelocity: Previous timestep velocity in the world frame
imuData: Current Readings of IMU Data

Returns:

A list containing velocity_x: current x direction velocity in the world frame
velocity_y: current y direction velocity in the world frame
velocity_z: current z direction velocity in the world frame