Add PID controller code

.gitignore

@@ -0,0 +1,2 @@
+.vscode/*
+venv/*

Controller/PID/co_constants.py

@@ -0,0 +1,21 @@
+# Author: Ajay Tak
+# Date: 28-06-2022
+
+import datetime as dt
+
+KR = 51.6393
+B = 3.837*1e-6
+I_1 = 0.0756
+I_2 = 0.0763
+I_3 = 0.0209
+J_PAR = 51.16e-6
+J_PER = (51.16e-6)/2
+MASS = 0.1
+H = 1e-2
+W_EARTH = 7.2921150e-5
+EPOCH = dt.datetime(2020, 1, 25, 12, 50, 19)	#date of launch t=0
+EQUINOX = dt.datetime(2019, 9, 23, 13, 20, 00)	#day of equinox
+STEPRUT = 1.002738 #sidereal time = stperut * universal time
+
+
+

Controller/PID/co_control_variable.py

@@ -0,0 +1,38 @@
+# Author: Ajay Tak
+# Date: 26-06-2022
+
+import numpy as np
+
+def co_state(q_actual, q_command):
+
+    """
+    > q_actual: The quaternion that represents a rotation from current/actual body frame to the inertial frame.
+    > q_command: The quaternion that represents a rotation from the desired orientation of the  body frame to the inertial frame.
+    """
+
+    M = np.matrix([[  q_command[0],  q_command[1],  q_command[2],  q_command[3]],
+                    [-q_command[1],  q_command[0], -q_command[3],  q_command[2]],
+                    [-q_command[2],  q_command[3],  q_command[0], -q_command[1]],
+                    [-q_command[3], -q_command[2],  q_command[1],  q_command[0]]])
+    q_e = np.dot(M, q_actual)
+    sigma_r = (2*q_e[0, 0])*np.array([q_e[0, 1], q_e[0, 2], q_e[0, 3]])
+    return sigma_r
+
+def co_current(q_actual, q_command, w_actual, w_command, h, sigma_integrate_prev, Kp, Ki, Kd):
+
+    """
+    > q_actual: The quaternion that represents a rotation from current/actual body frame to the inertial frame.
+    > q_command: The quaternion that represents a rotation from the desired orientation of the  body frame to the inertial frame.
+    > w_actual: The current/actual angular velocity of the body frame of the cubesat w.r.t the inertial frame.
+    > w_command: The required angular velocity of the body frame of cubesat w.r.t to inertial frame.
+    > h: time step
+    > Kp: The prortional gain matrix
+    > Kd: The derivative gain matrix
+    > Kd: The integrator gain matrix
+    > sigma_integrate_prev: integral of sigma from 0 to prev step
+    """
+
+    u = np.array((np.dot(Kp, co_state(q_actual, q_command)))+(np.dot(Kd, w_command-w_actual))+(np.dot(Ki,sigma_integrate_prev + h*co_state(q_actual, q_command))))
+    return u
+
+

Controller/PID/co_frames.py

@@ -0,0 +1,61 @@
+
+import numpy as np
+from math import radians, sin, cos, acos, pi
+from co_constants import W_EARTH, EPOCH, EQUINOX, STEPRUT
+
+def ned2ecef(v,lat,lon):
+	#rotate vector from North-East-Down frame to ecef
+	#Input:
+	#	lat: latittude in degrees ranges from -90 to 90
+	#	lon: longitude in degrees ranges from (-180,180]	 
+	if lat == 90 or lat == -90:
+		raise ValueError('Latittude value +/-90 occured. NED frame is not defined at north and south pole !!')
+	theta = -lat + 90. #in degree, polar angle (co-latitude)
+
+	if lon<0:
+		phi = 360. - abs(lon)
+	else:
+		phi = lon #in degree, azimuthal angle
+	theta = radians(theta)
+	phi = radians(phi)
+
+	m_DCM_n2e = np.array([[ -cos(theta)*cos(phi),	-sin(phi),	-sin(theta)*cos(phi)],
+						[	-cos(theta)*sin(phi),	cos(phi),	-sin(theta)*sin(phi)],
+						[	sin(theta),	0.0,	-cos(theta)]])
+
+	y = np.dot(m_DCM_n2e,v)
+
+	return y
+
+def ecef2ecif(v_x_e):
+	#Input ecef cector
+	# output ecif vector
+	ut_sec = (EPOCH - EQUINOX).total_seconds()# universal time vector in sec
+	theta = W_EARTH*ut_sec #in radian
+	m_DCM = np.array([[cos(theta), -1*sin(theta), 0.], [sin(theta), cos(theta),0.],[ 0.,0.,1.]])
+	v_x_i = np.dot(m_DCM,v_x_e)
+
+	return v_x_i
+
+def ecif2orbit(v_pos_i,v_vel_i,v_x_i):
+	#Input: v_pos_i is position in eci frame , v_vel_i is velocity in eci frame, v_x_i is vector to be transformed
+	#output: vector components in orbit frame
+	z = -v_pos_i/np.linalg.norm(v_pos_i)
+	y = np.cross(v_vel_i,v_pos_i)/np.linalg.norm(np.cross(v_vel_i,v_pos_i))
+	x = np.cross(y,z)/np.linalg.norm(np.cross(y,z))
+	m_DCM_OI = np.array([x,y,z])
+	v_x_o = np.dot(m_DCM_OI,v_x_i)
+
+	return v_x_o
+
+def orbit2body(v_i, roll, pitch, yaw):
+	# v_i the vector which need to be transformed from orbit to body frame
+	# roll, pitch, yaw are euler angles
+
+	m_tf = np.matrix([[np.cos(yaw)*np.cos(pitch), np.sin(yaw)*np.cos(pitch), -np.sin(pitch)],
+					  [(np.cos(yaw)*np.sin(pitch)*np.sin(roll))-(np.sin(yaw)*np.cos(roll)), (np.sin(yaw)*np.sin(pitch)*np.sin(roll))+(np.cos(yaw)*np.cos(roll)), np.cos(pitch)*np.sin(roll)],
+					  [(np.cos(yaw)*np.sin(pitch)*np.cos(roll))+(np.sin(yaw)*np.sin(roll)), (np.sin(yaw)*np.sin(pitch)*np.cos(roll))-(np.cos(yaw)*np.sin(roll)), np.cos(pitch)*np.cos(roll)]])
+	v = np.dot(m_tf, v_i)
+	v_b = np.array([v[0, 0], v[0, 1], v[0, 2]])
+
+	return v_b

Controller/PID/co_magnetic_field.py

@@ -0,0 +1,45 @@
+# Author: Ajay Tak
+# Date: 31-07-2022
+
+from turtle import shape
+from pyIGRF import *
+from co_frames import *
+import numpy as np
+
+time = np.arange(0, 54000, 0.1)
+omega_orbit = (2*np.pi)/5400  # T = 5400 sec is the assumed time period of our satellite in a orbit
+theta_orbit = omega_orbit*time # value of theta at different time steps
+r = 400 # altitude in km
+v = 400000*omega_orbit # magnitude of velocity of cubesat in m/s
+x = r*np.cos(theta_orbit) # x-component in perifocal plane
+y = r*np.sin(theta_orbit) # y-component in perifocal plane
+inc = 0 # inclination of orbit in radians
+X = x # x-component in ECIF
+Y = y*np.cos(inc) # y-component in ECIF
+Z = y*np.sin(inc) # z-component in ECIF
+v_X = -v*np.sin(theta_orbit)
+v_Y = v*np.cos(theta_orbit)
+v_Z = np.zeros_like(v_X)
+lat = (180/np.pi)*np.arcsin(Z/r) # latitude array at different time steps in degrees
+long = (180/np.pi)*np.arctan2(Y, X) # longitude array at different time steps in degrees
+date = 2020.25
+B = []
+for i in range(len(time)):
+    B.append(igrf_value(lat[i], long[i], 400, date)[3:6]) # magnetic field in NED frame in nT
+
+for i in range(len(time)):
+    B[i] = ned2ecef(B[i], lat[i], long[i]) # magnetic field in ECEF frame in nT
+
+for i in range(len(time)):
+    B[i] = ecef2ecif(B[i]) # magnetic field in ECI frame in nT
+
+for i in range(len(time)):
+    B[i] = ecif2orbit(np.array([X[i], Y[i], Z[i]]), np.array([v_X[i], v_Y[i], v_Z[i]]), (1e-9)*B[i]) # magnetic field in Orbit frame in T
+
+B = np.array(B)
+print(np.size(B))
+np.savetxt('magfield.csv', B)
+
+
+
+

Controller/PID/co_magnetorquers.py

@@ -0,0 +1,19 @@
+# Author: Ajay Tak
+# Date: 25-07-2022
+
+import numpy as np
+
+def co_actuator_model(M, B):
+
+    """
+    > M: The magnetic moment generated by magnetorquers
+    > B: Earth's Magnetic field expressed in cubesat's body frame
+    """
+
+    M_cross = np.matrix([[    0, -M[2],  M[1]],
+                         [ M[2],     0, -M[0]],
+                         [-M[1],  M[0],     0]])
+    multiply = np.dot(M_cross, B)
+    T = np.array([multiply[0, 0], multiply[0, 1], multiply[0, 2]])
+
+    return T

Controller/PID/co_main.py

@@ -0,0 +1,81 @@
+# Author: Ajay Tak
+# Date: 28-06-2022
+
+import numpy as np
+import matplotlib.pyplot as plt
+from co_propagating_w import *
+from co_propagating_q import *
+from co_magnetorquers import *
+from co_control_variable import *
+from co_quaternion_to_euler import *
+from co_sort import *
+from co_frames import *
+import co_constants
+
+omega_n = float(input("enter the value for omega_n: "))
+zeta = float(input("enter the value for zeta: "))
+timec  = float(input("enter the value for time constant: "))
+plot_graphs = input("please enter 1 for Yes, and 0 for no only: ")
+
+w = np.array([0.008726, 0.008726, 0.008726]) # initial angular velocity of the satellite (rad/s)
+s = np.array([0, 0, 0]) # initial control Torque (A)
+q = np.array([-0.3061862, 0.4355957, -0.6597396, 0.5303301]) # initial attitude
+q_command = np.array([1, 0, 0, 0])
+w_command = np.array([0, 0, 0])
+w_count = []
+q_count = []
+euler_plot = []
+sigma_integrate = [0, 0 , 0]
+K_p = np.dot(((omega_n**2)+((2*0.015*zeta*omega_n)/timec)), np.diag(np.array([co_constants.I_1, co_constants.I_2, co_constants.I_3])))
+K_d = np.dot(((2*zeta*omega_n)+(0.015/timec)), np.diag(np.array([co_constants.I_1, co_constants.I_2, co_constants.I_3])))
+K_i = np.dot(0.015*((omega_n**2)/timec), np.diag(np.array([co_constants.I_1, co_constants.I_2, co_constants.I_3])))
+mag_field = np.genfromtxt('magfield.csv')
+
+
+for i in range(270000):
+    w_count.append(w)
+    q_count.append(q)
+    B_orbit = mag_field[i]
+    euler_angles = co_quat_to_euler(q)
+    euler_plot.append(euler_angles)
+    B_body = orbit2body(B_orbit, euler_angles[2], euler_angles[1], euler_angles[0])
+    i_control_variable = co_current(q, q_command, w, w_command, co_constants.H, sigma_integrate, K_p, K_i, K_d)*1e-2
+    T = co_actuator_model(i_control_variable, B_body)
+    w_new = co_propagating_w_i(w[0], w[1], w[2], co_constants.I_1, co_constants.I_2, co_constants.I_3, T[0], T[1], T[2], co_constants.H)
+    q_new = co_propagatin_q_IB(q[0], q[1], q[2], q[3], w[0], w[1], w[2], co_constants.H)
+    sigma_integrate = sigma_integrate + (co_constants.H*co_state(q, q_command))
+    w = w_new
+    q = q_new
+    print(np.sqrt((q[0]**2)+(q[1]**2)+(q[2]**2)+(q[3]**2)))
+
+if plot_graphs:
+    t = np.arange(0, 27000, 0.1)   
+    [w_x_plot, w_y_plot, w_z_plot] = co_sorting(w_count)
+    [yaw_plot, pitch_plot, roll_plot] = co_sorting(euler_plot)
+    plt.figure(1)
+    plt.subplot(2, 3, 1)
+    plt.xlabel("time")
+    plt.ylabel("w_x")
+    plt.plot(t, w_x_plot)
+    plt.subplot(2, 3, 2)
+    plt.xlabel("time")
+    plt.ylabel("w_y")
+    plt.plot(t, w_y_plot)
+    plt.subplot(2, 3, 3)
+    plt.xlabel("time")
+    plt.ylabel("w_z")
+    plt.plot(t, w_z_plot)
+    plt.subplot(2, 3, 4)
+    plt.xlabel("time")
+    plt.ylabel("roll")
+    plt.plot(t, roll_plot)
+    plt.subplot(2, 3, 5)
+    plt.xlabel("time")
+    plt.ylabel("pitch")
+    plt.plot(t, pitch_plot)
+    plt.subplot(2, 3, 6)
+    plt.xlabel("time")
+    plt.ylabel("yaw")
+    plt.plot(t, yaw_plot)
+    plt.show()
+    print([roll_plot[-1], pitch_plot[-1], yaw_plot[-1]])

Controller/PID/co_propagating_q.py

@@ -0,0 +1,43 @@
+# Author: Ajay Tak
+# Date: 14-06-2022
+
+import numpy as np
+
+def co_kinematic_function(q_0, q_1, q_2, q_3, P, Q, R):
+
+    """
+    > q_0, q_1, q_2, q_3 are the components of quaternion from body frame to inertial frame
+    > P, Q, R are the components of the angular velocity of body frame with respect to inertial frame
+    """
+
+    q_IB = np.array([q_0, q_1, q_2, q_3])
+    w = np.array([P, Q, R])
+    m_Q = 0.5*np.matrix([[-q_IB[1], -q_IB[2], -q_IB[3]],
+                         [ q_IB[0], -q_IB[3],  q_IB[2]],
+                         [ q_IB[3],  q_IB[0], -q_IB[1]],
+                         [-q_IB[2],  q_IB[1],  q_IB[0]]])
+    dot = np.dot(m_Q, w)
+    q_IB_dot = np.array([dot[0, 0], dot[0, 1], dot[0, 2], dot[0, 3]])
+
+    return q_IB_dot
+
+def co_propagatin_q_IB(q_0, q_1, q_2, q_3, P, Q, R, h):
+
+    """
+    > q_0, q_1, q_2, q_3 are the components of quaternion from body frame to inertial frame
+    > P, Q, R are the components of the angular velocity of body frame with respect to inertial frame
+    > h is time step
+    """
+
+    q_IB = np.array([q_0, q_1, q_2, q_3])
+    a = co_kinematic_function(q_0, q_1, q_2, q_3, P, Q, R)
+    b = co_kinematic_function(q_0+(0.5*h*a[0]), q_1+(0.5*h*a[1]), q_2+(0.5*h*a[2]), q_3+(0.5*h*a[3]), P, Q, R)
+    c = co_kinematic_function(q_0+(0.5*h*b[0]), q_1+(0.5*h*b[1]), q_2+(0.5*h*b[2]), q_3+(0.5*h*b[3]), P, Q, R)
+    d = co_kinematic_function(q_0+(h*c[0]), q_1+(h*c[1]), q_2+(h*c[2]), q_3+(h*c[3]), P, Q, R)
+    q_IB_next = q_IB + ((h/6)*(a + 2*b + 2*c + d))
+    """if q_IB_next[0] > 0:
+        q_IB_next[0] = np.sqrt(1-(q_IB[1]**2)-(q_IB[2]**2)-(q_IB[3]**2))
+    else:
+        q_IB_next[0] = -np.sqrt(1-(q_IB[1]**2)-(q_IB[2]**2)-(q_IB[3]**2))"""
+
+    return q_IB_next

Controller/PID/co_propagating_w.py

@@ -0,0 +1,44 @@
+# Author: Ajay Tak
+# Date: 25-07-2022
+
+import numpy as np
+
+def co_dynamic_function(I_i1, I_i2, I_i3, T_i1, w_i2, w_i3):
+
+    """
+    > T_i1 is one of the component of the torque provided by magnetorquer
+    > w_i2, w_i3 are  2 components of cubesat's angular velocity wrt to earth's inertial frame expressed in body frame
+    > I_i1, I_i2, I_i3 are the principle elements of the moment of inertia matrix of the cubesat
+    """
+
+    w_i1_dot = ((w_i2*w_i3*(I_i2-I_i3)) + T_i1)/I_i1
+
+    return w_i1_dot
+
+
+def co_propagating_w_i(w_1, w_2, w_3, I_1, I_2, I_3, T_1, T_2, T_3, h):
+
+    """
+    > w_1, w_2, w_3 are components of cubesat's angular velocity wrt to earth's inertial frame expressed in body frame
+    > I_1, I_2, I_3 are the principle elements of the moment of inertia matrix
+    > T_1, T_2, T_3 are the three components of the torque provided by the magnetorquers
+    > h is time step
+    """
+
+    a1 = co_dynamic_function(I_1, I_2, I_3, T_1, w_2, w_3)
+    a2 = co_dynamic_function(I_2, I_3, I_1, T_2, w_3, w_1)
+    a3 = co_dynamic_function(I_3, I_1, I_2, T_3, w_1, w_2)
+    b1 = co_dynamic_function(I_1, I_2, I_3, T_1, w_2 + (0.5*h*a2), w_3 + (0.5*h*a3))
+    b2 = co_dynamic_function(I_2, I_3, I_1, T_2, w_3 + (0.5*h*a3), w_1 + (0.5*h*a1))
+    b3 = co_dynamic_function(I_3, I_1, I_2, T_3, w_1 + (0.5*h*a1), w_2 + (0.5*h*a2))
+    c1 = co_dynamic_function(I_1, I_2, I_3, T_1, w_2 + (0.5*h*b2), w_3 + (0.5*h*b3))
+    c2 = co_dynamic_function(I_2, I_3, I_1, T_2, w_3 + (0.5*h*b3), w_1 + (0.5*h*b1))
+    c3 = co_dynamic_function(I_3, I_1, I_2, T_3, w_1 + (0.5*h*b1), w_2 + (0.5*h*b2))
+    d1 = co_dynamic_function(I_1, I_2, I_3, T_1, w_2 + (h*c2), w_3 + (h*c3))
+    d2 = co_dynamic_function(I_2, I_3, I_1, T_2, w_3 + (h*c3), w_1 + (h*c1))
+    d3 = co_dynamic_function(I_3, I_1, I_2, T_3, w_1 + (h*c1), w_2 + (h*c2))
+    w_1_next = w_1 + ((h/6)*(a1 + (2*b1) + (2*c1) + (d1)))
+    w_2_next = w_2 + ((h/6)*(a2 + (2*b2) + (2*c2) + (d2)))
+    w_3_next = w_3 + ((h/6)*(a3 + (2*b3) + (2*c3) + (d3)))
+
+    return np.array([w_1_next, w_2_next, w_3_next])

Controller/PID/co_quaternion_to_euler.py

@@ -0,0 +1,9 @@
+# Author: Ajay Tak
+# Date: 30-06-2022
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+def co_quat_to_euler(q):
+    Q = Rotation.from_quat([q[1], q[2], q[3], q[0]])
+    return Q.as_euler('zyx', True)

Controller/PID/co_sort.py

@@ -0,0 +1,14 @@
+# Author: Ajay Tak
+# Date: 30-06-2022
+
+def co_sorting(arr):
+    l = len(arr)
+    arr1 =[]
+    arr2 =[]
+    arr3 =[]
+    for i in range(l):
+        arr1.append(arr[i][0])
+        arr2.append(arr[i][1])
+        arr3.append(arr[i][2])
+    return [arr1, arr2, arr3]
+