Error in formulation of a Derivative(PD) controller

Question

I'm applying Proportional-Derivative controller to control my model in ROS. I'm limited to python 2.7.17 version.
There are two types of errors in this script; position error(ep) and heading error(eth).

I've given last_error=0 and trying to get the updation in (ep_dot) and (eth_dot) as a method to find derivative of error. I wonder whether my given formula for derivative(ep_dot) and (eth_dot) is correct or not.Is this correct way of finding derivative ? Is there any other relevant approach to find the same? Kindly let me know the solution.

#!/usr/bin/env python
import rospy 
import math 
import csv
import time
from time import gmtime,strftime
from datetime import datetime
import numpy as np      # for converting radians to degrees
from geometry_msgs.msg import Twist #to obtain command velocities
from nav_msgs.msg import Odometry #to obtain position and orientation
from tf.transformations import euler_from_quaternion, quaternion_from_euler

roll = pitch = yaw = 0.0

t_start = time.time()
**last_error =0**

def get_rotation(msg):
    velo_msg  = Twist()
    global roll,pitch,yaw
    orientation_q = msg.pose.pose.orientation
    orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
    (roll, pitch, yaw) = euler_from_quaternion(orientation_list)

    kpp = 0.74
    kpth = 0.5
    kd = 0.1
    Tmax = 60

    t_milli = (time.time() - t_start)*1000
    t = t_milli/1000  # to get the values in seconds
    print("t=",t)
    if t > Tmax: 
        rospy.signal_shutdown("Simulation ends here!")
      
    x_now = msg.pose.pose.position.x
    y_now = msg.pose.pose.position.y
    th = yaw
    xT = math.cos(2*math.pi*t/Tmax)*0.8
    yT = math.sin(2*math.pi*t/Tmax)*0.8  

    #Trasformation matrix # Finding INVERSE                           
    I = np.array([[math.cos(th), -math.sin(th), x_now],[math.sin(th), math.cos(th), y_now],[0, 0, 1]])
    B = np.array([[xT],[yT],[1]])      # print('B=',B)
    C = np.dot(np.linalg.inv(I),B) # np.dot: for matrix multiplication
    xTB = C[0][0]  # [] indexing to extract the values of an array/matrix
    yTB = C[1][0]
    

    ep = math.sqrt(xTB*xTB + yTB*yTB)     # error calc.
    ep_dot = (ep-last_error)/t      
    eth = math.atan2(yTB,xTB) 
    eth_dot = (eth-last_error)/t

    print('ep =',ep,'eth(deg)=',eth*180/math.pi,'radius=',math.sqrt(x_now*x_now + y_now*y_now),'t=',t)
    
    PID_lin = ep*kpp + ep_dot*kd 
    PID_ang = eth*kpth - eth_dot*kd
   
    # publishing the cmd_vel in linear and angular motion both
    velo_msg.linear.x = PID_lin 
    velo_msg.angular.z = PID_ang
    pub.publish(velo_msg)
    

rospy.init_node('shadan')
sub = rospy.Subscriber("/odom", Odometry, get_rotation)
pub = rospy.Publisher('/cmd_vel',Twist,queue_size=10)
r = rospy.Rate(10)
while not rospy.is_shutdown():
    r.sleep()

Answer 1

I think there is an issue with your calculation of the derivative terms.

You get t_start=time.time() at the very beginning of your code and then every time you get in your callback you update t_milli = (time.time() - t_start)*1000 and t = t_milli/1000 with t_start being a constant.

Then you calculate ep_dot as being equals to ep_dot = (ep-last_error)/t. But ep_dot should be equal to something like ep_dot = (actual_value - last_value)/(t_actual - t_last_value)

The derivative term should be equal to the difference of two consecutive values divided by the difference of time between those two values so you need to store the time of the previous data acquired in the callback in a new variable.

Also, the calculation of the proportional error should be equal to your desired value (goal) minus the actual value. Then you multiply that by your P coefficient.

Hope it fixes your problem !