Dead Reckoning 2d

by Daniel Pollithy

Dead reckoning

The following notebook contains a simple implementation of dead reckoning with the Explicit and Implicit Euler Method for a bi-cycle given the steering angle and velocity at every timestep t.


import math
import numpy as np
import matplotlib.pyplot as plt

Explicit Euler Method

where T is the discretization step size.

The derivative can be approximated by a circular path around the instant center of rotation “ICR”).


def explicit_euler(length, t_end, T, X_start, v_func, alpha_func):
    # explicit euler
    explicits = [X_start]
    
    t_steps = int(math.ceil((t_end - 0.0) / T))
    
    for i in range(t_steps):
        # velocity and steer at time i
        v_k = v_func(i)
        alpha_k = alpha_func(i) if alpha_func(i) != 0 else 0.00001
        R = length / math.tan(alpha_k)

        # old X and derivative of old X
        x_k, y_k, theta_k = explicits[-1]
        x_dot = v_k * np.array([math.cos(theta_k), math.sin(theta_k), 1/R])

        # store values and norm angles only between 0 and 2 pi
        explicits += [ explicits[-1] + T * x_dot] 
        explicits[-1][2] %= 2*math.pi

    np_explicits = np.array(explicits)
    
    return np_explicits

Implicit Euler Method

yields system of linear equations.

The third equation yields $ \theta_{k+1} $ which can be inserted into the first and second.


def implicit_euler(length, t_end, T, X_start, v_func, alpha_func):
    implicits = [X_start]
    t_steps = int(math.ceil((t_end - 0.0) / T))
    
    for i in range(t_steps):
        # old state
        x_k, y_k, theta_k = implicits[-1]
        
        # next control variables
        alpha_k_plus_1 = alpha_func(i+1)
        v_k_plus_1 = v_func(i+1)
        
        # X_k+1
        theta_k_plus_1 = theta_k + T * v_k_plus_1 * 2 * math.tan(alpha_k_plus_1)
        x_k_plus_1 = x_k + T * v_k_plus_1 * math.cos(theta_k_plus_1)
        y_k_plus_1 = y_k + T * v_k_plus_1 * math.sin(theta_k_plus_1)

        # store values and norm angles only between 0 and 2 pi
        implicits += [ np.array([x_k_plus_1, y_k_plus_1, theta_k_plus_1]) ] 
        implicits[-1][2] %= 2*math.pi

    np_implicits = np.array(implicits)
    
    return np_implicits

def visualize_state_sequence(Xs):
    plt.plot(range(Xs.shape[0]), Xs)
    plt.show()
    plt.scatter(Xs[:, [0]], Xs[:, [1]]) 
    plt.show()

Test the functions

Assume a constant velocity and a stair function for steering angles.


# command variables
def v(t):
    return 2

def alpha(t):
    if t == 0:
        return 0.001
    if 1 < t < 2:
        return 0.3 * math.pi / 180.0
    
    return 0.15 * math.pi / 180.0

x1 = explicit_euler(length=0.5, t_end=3.0, T=0.1, X_start=np.zeros(3), v_func=v, alpha_func=alpha)

visualize_state_sequence(x1)

png

png


x2 = implicit_euler(length=0.5, t_end=3.0, T=0.1, X_start=np.zeros(3), v_func=v, alpha_func=alpha)

visualize_state_sequence(x2)

png

png


x3 = explicit_euler(length=0.5, t_end=15.0, T=0.1, X_start=np.zeros(3), v_func=v, alpha_func=math.sin)

visualize_state_sequence(x3)

png

png


x4 = implicit_euler(length=0.5, t_end=15.0, T=0.1, X_start=np.zeros(3), v_func=v, alpha_func=math.sin)

visualize_state_sequence(x4)

png

png