Code_Problem_Set_1_Math.py

import numpy as np
from scipy.signal import cont2discrete
from scipy.linalg import expm
import matplotlib.pyplot as plt

# Given parameters
m = 1
b = 0.1
h = 0.01

# Continuous-time state-space matrices
A = -b/m
B = 1/m
C = 1
D = 0

# Analytic Calculation
Ad_analytic = np.exp(-b/m * h)
Bd_analytic = (1 - np.exp(-b/m * h)) / b
print(f'Analytic AD: {Ad_analytic}')
print(f'Analytic BD: {Bd_analytic}')

# Matrix Exponential Method
block_matrix = np.array([[A, B], [0, 0]]) * h
expm_result = expm(block_matrix)
Ad_expm = expm_result[0, 0]
Bd_expm = expm_result[0, 1]
print(f'Matrix Exponential AD: {Ad_expm}')
print(f'Matrix Exponential BD: {Bd_expm}')

# Using c2d()
A_c2d = np.array([[A]])
B_c2d = np.array([[B]])
C_c2d = np.array([[C]])
D_c2d = np.array([[D]])
Ad_c2d, Bd_c2d, _, _, _ = cont2discrete((A_c2d, B_c2d, C_c2d, D_c2d), h, method='zoh')
print(f'c2d (cont2discrete) AD: {Ad_c2d[0][0]}')
print(f'c2d (cont2discrete) BD: {Bd_c2d[0][0]}')


# Discrete-time matrices from analytic solution
Ad = np.exp(-b/m * h)
Bd = (1 - np.exp(-b/m * h)) / b

# Define the simulation parameters
k = 10000
time_v = np.arange(0, k) * h
ut_v = np.ones(k)
velocity_v = np.zeros(k)
output_v = np.zeros(k)

# Set the initial condition
velocity_v[0] = 0

# Simulate the system
for k in range(k - 1):
    # Update the state
    velocity_v[k+1] = Ad * velocity_v[k] + Bd * ut_v[k]
    
    # Calculate the output
    output_v[k] = velocity_v[k]
"""
# Plot the velocity
plt.figure()
plt.plot(time_v, output_v, 'b', linewidth=2)
plt.title("Car Velocity over Time")
plt.xlabel("Time (s)")
plt.ylabel("Velocity (m/s)")
plt.grid(True)
plt.box(True)
plt.show()
"""

# Define New friction coefficients 
b_model = 0.1
b_real = 0.4

# Ad and Bd for b model 
Ad_model = np.exp(-b_model/m * h)
Bd_model = (1 - np.exp(-b_model/m * h)) / b_model

# Ad and Bd for b real
Ad_real = np.exp(-b_real/m * h)
Bd_real = (1 - np.exp(-b_real/m * h)) / b_real

# Simulation parameters
k = 10000
time_v = np.arange(0, k) * h
ut_v = np.ones(k)

# Initialize velocity vectors for both systems
velocity_v_model = np.zeros(k)
velocity_v_real = np.zeros(k)

# Set the initial conditions
velocity_v_model[0] = 0
velocity_v_real[0] = 0

# Simulate both systems
for k in range(k - 1):
    # Update the state for the model system
    velocity_v_model[k+1] = Ad_model * velocity_v_model[k] + Bd_model * ut_v[k]
    
    # Update the state for the real-world system
    velocity_v_real[k+1] = Ad_real * velocity_v_real[k] + Bd_real * ut_v[k]

# Calculate the error between the real and model outputs
# Note: The output yk is equal to the velocity vk for both systems
error_v = velocity_v_real - velocity_v_model
"""
# Plot the error
plt.figure()
plt.plot(time_v, error_v, 'r', linewidth=2)
plt.title('Error between Real and Model Outputs')
plt.xlabel('Time (s)')
plt.ylabel('Error (m/s)')
plt.grid(True)
plt.box(True)
plt.show()
"""



# Parameters
m = 1
b = 0.1
h = 0.01
g = 10
theta_deg = 30
theta_rad = np.deg2rad(theta_deg)

#AD and BD
Ad = np.exp(-b/m * h)
Bd = (1 - np.exp(-b/m * h)) / b

# Term from gravity
Fd = m * g * np.sin(theta_rad) * Bd

# Define the simulation parameters
k = 10000
time_v = np.arange(0, k) * h
ut_v = np.ones(k)

# Initialize velocity vectors for both systems
velocity_v_g = np.zeros(k)
velocity_v_no_g = np.zeros(k)

# Set the initial condition
velocity_v_g[0] = 0
velocity_v_no_g[0] = 0

# Simulate the systems
for k in range(k - 1):
    # Update the state with the gravitational term
    velocity_v_g[k+1] = Ad * velocity_v_g[k] + Bd * ut_v[k] + Fd

    # Update the state without the gravitational term
    velocity_v_no_g[k+1] = Ad * velocity_v_no_g[k] + Bd * ut_v[k]

# Plot the velocities
plt.figure()
plt.plot(time_v, velocity_v_g, 'g', label='With Gravity', linewidth=2)
plt.plot(time_v, velocity_v_no_g, 'b--', label='Without Gravity', linewidth=2)
plt.title('Car Velocity Over Time')
plt.xlabel('Time (s)')
plt.ylabel('Velocity (m/s)')
plt.legend()
plt.grid(True)
plt.box(True)
plt.show()



# Error between the two output