Observer task object compatible with the cotask scheduler. More...

Public Member Functions
	__init__ (self, left_pos, right_pos, yaw, yaw_velocity, left_voltage, right_voltage, left_vel, right_vel, total_dist)
	Observer object initializer.
	update_x (self)
	Updates the state vector with real values and the last guessed total distance.
	update_u (self)
	Updates the input vector with real values assuming full battery voltage.
	update_y (self)
	Updates the output vector with real values.
	update_u_star (self)
	Updates the discretized combined input and output vector.
	calc_derivative (self, x, u)
	Calculates the derivative state vector using the continuous model with no feedback.
	calc_output (self, x, u)
	Calculates the output vector using the continuous model with no feedback.
	calc_discrete_state (self, x, u_star)
	Calculates the next state vector using the discretized model.
	calc_discrete_output (self, x)
	Calculates the current output vector using the discretized model.
	RK4_solver (self, x, t_step, u)
	RK4_solver is an implementation of the RK4 algorithm used to estimate the state when using a continuous model.
	run (self)
	Generator that the cotask scheduler will run since our tasks are objects.

Public Attributes
	left_pos = left_pos
	right_pos = right_pos
	yaw = yaw
	yaw_velocity = yaw_velocity
	left_voltage = left_voltage
	right_voltage = right_voltage
	left_vel = left_vel
	right_vel = right_vel
	total_dist = total_dist
	x = np.array([[0], [0], [0], [0]])
	u = np.array([[0], [0]])
	y = np.array([[0], [0], [0], [0]])
	u_star = np.array([[0], [0], [0], [0], [0], [0]])
	x_estimated = np.array([[0], [0], [0], [0]])
	u_estimated = np.array([[0], [0]])
	y_estimated = np.array([[0], [0], [0], [0]])
	u_star_estimated = np.array([[0], [0], [0], [0], [0], [0]])
	L
	A_d
	B_d
	C
int	state = 0
int	feedback = 1

Detailed Description

Observer task object compatible with the cotask scheduler.

This file contains a class to create an observer object which is compatible with the cotask scheduler to run as a cooperative task.

This task always estimates the state of ROMI given its sensor inputs using a discretized state space model.

NOTE: These values have errors and the errors get worse as ROMI runs; it is only an estimation.

IMPORTANT: This observer will reach a null space if the encoder values and yaw value are not possible with each other. If the encoder positions can't equal the yaw value of the sensors this will fail. This happens on start because the encoders start at zero but the yaw does not necessarily start at 0 as well. The correction for this is in state 0 of the control task where the encoders are given an offset to make them match whatever the starting yaw is.

Author: Alex Power; Lucas Heuchert; Erik Heuchert

Date: 2025-Nov-11 Approximate date of creation of file; 2025-Dec-12 Final alterations made

Definition at line 30 of file observer.py.

Constructor & Destructor Documentation

◆ init()

observer.observer.__init__	(	self,
		left_pos,
		right_pos,
		yaw,
		yaw_velocity,
		left_voltage,
		right_voltage,
		left_vel,
		right_vel,
		total_dist )

Observer object initializer.

The arguments are data shares.

Parameters

left_pos	Float share that holds the current position of the left encoder. Used as an input.
right_pos	Float share that holds the current position of the right encoder. Used as an input.
yaw	Float share that holds the IMU reading of the yaw.
yaw_velocity	Yaw velocity as measured by the IMU task.
left_voltage	PWM value of the left motor which observer scales to the expected voltage.
right_voltage	PWM value of the right motor which observer scales to the expected voltage.
left_vel	Velocity of the left motor.
right_vel	Velocity of the right motor.
total_dist	Float share that the observer will fill with how far it thinks ROMI has travelled in mm.

Definition at line 49 of file observer.py.

Member Function Documentation

◆ calc_derivative()

observer.observer.calc_derivative	(	self,
		x,
		u )

Calculates the derivative state vector using the continuous model with no feedback.

Parameters

x	State vector [4,1].
u	Input vector [2,1].

Returns

Derivative of the state vector.

Definition at line 148 of file observer.py.

Here is the caller graph for this function:

◆ calc_discrete_output()

observer.observer.calc_discrete_output	(		self,
			x )

Calculates the current output vector using the discretized model.

Parameters

x	State vector [4,1].

Returns

Output vector [4,1].

Definition at line 229 of file observer.py.

Here is the caller graph for this function:

◆ calc_discrete_state()

observer.observer.calc_discrete_state	(	self,
		x,
		u_star )

Calculates the next state vector using the discretized model.

Parameters

x	State vector [4,1].
u_star	Combined input and output vector [6,1].

Returns

Next state vector.

Definition at line 218 of file observer.py.

Here is the caller graph for this function:

◆ calc_output()

observer.observer.calc_output	(	self,
		x,
		u )

Calculates the output vector using the continuous model with no feedback.

Parameters

x	State vector [4,1].
u	Input vector [2,1].

Returns

Output vector [4,1].

Definition at line 186 of file observer.py.

Here is the caller graph for this function:

◆ RK4_solver()

observer.observer.RK4_solver	(	self,
		x,
		t_step,
		u )

RK4_solver is an implementation of the RK4 algorithm used to estimate the state when using a continuous model.

Parameters

x	Current state vector.
t_step	Time step between the current state and the estimated state. Should be the time when this task next runs.
u	Input vector.

Returns

Next state and current output as (x_next, y).

Definition at line 244 of file observer.py.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ run()

observer.observer.run ( self )

Generator that the cotask scheduler will run since our tasks are objects.

All shares it needs are already given to the task object on initialization. The generator will only run one state at a time on each run of the task.

The observer will always update its current states then guess at the next state. The only state vector it does not have direct data for is total distance travelled so it will always use its estimation for the next state.

If feedback is turned off the observer will run the simple state space continuous model that has no feedback. This is not really an observer, it is just an estimator.

If feedback is turned on the observer will run the discretized model of the system. Typically the total distance travelled is useful but the others are not. The value it predicts is also pretty consistent each run, but it is always negative and off by a factor of about 2.1 so we simply scale it by -2.1 so that the value is more useful. It is in mm.

Definition at line 285 of file observer.py.

Here is the call graph for this function:

◆ update_u()

observer.observer.update_u ( self )

Updates the input vector with real values assuming full battery voltage.

Definition at line 119 of file observer.py.

Here is the caller graph for this function:

◆ update_u_star()

observer.observer.update_u_star ( self )

Updates the discretized combined input and output vector.

Definition at line 131 of file observer.py.

Here is the caller graph for this function:

◆ update_x()

observer.observer.update_x ( self )

Updates the state vector with real values and the last guessed total distance.

Definition at line 111 of file observer.py.

Here is the caller graph for this function:

◆ update_y()

observer.observer.update_y ( self )

Updates the output vector with real values.

Definition at line 124 of file observer.py.

Here is the caller graph for this function:

Member Data Documentation

◆ A_d

observer.observer.A_d

Initial value:

=  np.array([
            [0.1668, 0.1668, -659.9169, -0.0000],
            [0.1668, 0.1668, -659.9169,  0.0000],
            [0.0000, 0.0000,   -0.1193,  0.0000],
            [0.0000, 0.0000,    0.0000,  0.0000]
        ])

Definition at line 83 of file observer.py.

◆ B_d

observer.observer.B_d

Initial value:

=  np.array([
            [0.4216, 0.0363, 329.9584, 329.9584, 0.0000, -1.9941],
            [0.3635, 0.0422, 329.9584, 329.9584, 0.0000,  1.9941],
            [0.0001, 0.0000,   0.5597,   0.5597, 0.0000,  0.0000],
            [0.0000, 0.0000,  -0.0698,   0.0698, 0.9902,  0.0010]
        ])

Definition at line 91 of file observer.py.

◆ C

observer.observer.C

Initial value:

=  np.array([
            [0, 0, 1, -0.141 / 2],
            [0, 0, 1,  0.141 / 2],
            [0, 0, 0,  1],
            [-0.035 / 0.141, 0.035 / 0.141, 0, 0]
        ])

Definition at line 99 of file observer.py.

◆ feedback

int observer.observer.feedback = 1

Definition at line 107 of file observer.py.

◆ L

observer.observer.L

Initial value:

=  np.array([
            [2.2683e+05, 2.2683e+05, 1.2069e-08, -1.9921e+03],
            [2.2683e+05, 2.2683e+05, 1.2068e-08,  1.9921e+03],
            [9.5000e+01, 9.5000e+01, -2.0531e-11, 9.8455e-10],
            [-6.98061e+01, 6.98061e+01, 9.901573e+02, 1.0000]
        ])

Definition at line 75 of file observer.py.

◆ left_pos

observer.observer.left_pos = left_pos

Definition at line 52 of file observer.py.

◆ left_vel

observer.observer.left_vel = left_vel

Definition at line 58 of file observer.py.

◆ left_voltage

observer.observer.left_voltage = left_voltage

Definition at line 56 of file observer.py.

◆ right_pos

observer.observer.right_pos = right_pos

Definition at line 53 of file observer.py.

◆ right_vel

observer.observer.right_vel = right_vel

Definition at line 59 of file observer.py.

◆ right_voltage

observer.observer.right_voltage = right_voltage

Definition at line 57 of file observer.py.

◆ state

int observer.observer.state = 0

Definition at line 106 of file observer.py.

◆ total_dist

observer.observer.total_dist = total_dist

Definition at line 60 of file observer.py.

◆ u

observer.observer.u = np.array([[0], [0]])

Definition at line 64 of file observer.py.

◆ u_estimated

observer.observer.u_estimated = np.array([[0], [0]])

Definition at line 70 of file observer.py.

◆ u_star

observer.observer.u_star = np.array([[0], [0], [0], [0], [0], [0]])

Definition at line 66 of file observer.py.

◆ u_star_estimated

observer.observer.u_star_estimated = np.array([[0], [0], [0], [0], [0], [0]])

Definition at line 72 of file observer.py.

◆ x

observer.observer.x = np.array([[0], [0], [0], [0]])

Definition at line 63 of file observer.py.

◆ x_estimated

observer.observer.x_estimated = np.array([[0], [0], [0], [0]])

Definition at line 69 of file observer.py.

◆ y

observer.observer.y = np.array([[0], [0], [0], [0]])

Definition at line 65 of file observer.py.

◆ y_estimated

observer.observer.y_estimated = np.array([[0], [0], [0], [0]])

Definition at line 71 of file observer.py.

◆ yaw

observer.observer.yaw = yaw

Definition at line 54 of file observer.py.

◆ yaw_velocity

observer.observer.yaw_velocity = yaw_velocity

Definition at line 55 of file observer.py.

The documentation for this class was generated from the following file:

C:/Users/14083/vscode_python/observer.py

Public Member Functions

Public Attributes

Detailed Description

Constructor & Destructor Documentation

◆ __init__()

Member Function Documentation

◆ calc_derivative()

◆ calc_discrete_output()

◆ calc_discrete_state()

◆ calc_output()

◆ RK4_solver()

◆ run()

◆ update_u()

◆ update_u_star()

◆ update_x()

◆ update_y()

Member Data Documentation

◆ A_d

◆ B_d

◆ C

◆ feedback

◆ L

◆ left_pos

◆ left_vel

◆ left_voltage

◆ right_pos

◆ right_vel

◆ right_voltage

◆ state

◆ total_dist

◆ u

◆ u_estimated

◆ u_star

◆ u_star_estimated

◆ x

◆ x_estimated

◆ y

◆ y_estimated

◆ yaw

◆ yaw_velocity

◆ init()