weird airsim ros position data leading to interesting results when building a PID controller

Hello! So originally built a PID controller for airsim drone by just using the: pos = client.getMultirotorState().kinematics_estimated.position data the PID controller is only for the Z axis.

I now I would like to do the same with ROS position data from imu from odometry (specifically the X, Y, Z positions). But the results I am getting are absolutely weird. Not only are the positions from the: airsim_node/Drone1/odom_local_ned

not accurate (it keeps oscillating around the start position value of the drone) but when I execute the program the drone stays on the floor even though its Z_V is very large. After a long while it then shoots up in the air and never stops. But also weirdly enough the drone moves in the X and Y axises even though in: client.moveByVelocityAsync(0,0,Z_V,timepause) The X and Y velocities are 0.

Here is my code:

#!/usr/bin/env python
import sys
sys.path.append('/home/edwin-pcp/AirSim/PythonClient/multirotor') #path to setup_path file
import setup_path 
import airsim
import rospy
import numpy as np
import os
import tempfile
import cv2
import math
import time
from nav_msgs.msg import Odometry
import matplotlib.pyplot as plt

class Position:
    def __init__(self, pos):
        self.x = pos.x_val
        self.y = pos.y_val
        self.z = pos.z_val

Z_diff = 0
Z_diff1 = 0
intticker = 0
Perror = 0
Ierror = 0
Derror =  0
timepause = 1
error = 0.01
tickval = 0
done = 0
ticker = 0
data = []
#official position wanted (will not be changed throughout)
Poswanted = -10
#pivals = np.arange(0,2.01,0.01)
client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)   
client.armDisarm(True)
client.takeoffAsync().join()

#X_pos = 0
#Y_pos = 0
#Z_pos = 0

client.moveToPositionAsync(-10, 10, -20, 5).join()

def callback(data):
#   global X_pos
#   global Y_pos
#   global Z_pos
    X_pos = data.pose.pose.position.x
    Y_pos = data.pose.pose.position.y
    Z_pos = data.pose.pose.position.z
    pos = client.getMultirotorState().kinematics_estimated.position
    print('X_pos pos.x_val',X_pos,pos.x_val)
    print('Y_pos pos.y_val',Y_pos,pos.y_val)
    print('Z_pos pos.z_val',Z_pos,pos.z_val)
    rospy.loginfo(rospy.get_caller_id() + 'I heard %s', data.pose.pose)
    PIDcontroller(X_pos,Y_pos,Z_pos)
    time.sleep(2)

def PIDcontroller(X_pos,Y_pos,Z_pos):
    global Ierror
    global Derror
    global Perror
    global Z_diff1
    global Z_diff
    global ticker
    global data

    #Z_pos = -1*Z_pos

    #setts up initially Z_diff value(will be change on each loop)
    Z_diff = Poswanted + Z_pos

    #initial print of data
    print("Z_pos: ",Z_pos)
    print("Z_diff: ",Z_diff)
    if (((Z_diff > error) or (Z_diff < error))):
        print("loop entered")

        #gains
        Kp = 40 #100
        Ki = 0.01
        Kd = 1

        #BTW Z_diff is error
        Ierror = Ierror + Z_diff*timepause

        Derror = (Z_diff - Z_diff1)/timepause

        Z_V = Kp*Z_diff + Ki * Ierror + Kd * Derror

        print("Z_V",Z_V)

        #velocity movement
        client.moveByVelocityAsync(0,0,Z_V,timepause)

        #new positioning

        Z_diff1 = Z_diff
        Z_diff = Poswanted + Z_pos
        print("Z_pos: ",Z_pos)
        print("Z_diff: ",Z_diff)
        print("Poswanted: ",Poswanted)
        print("Ierror",Ierror)
        print("Derror",Derror)

if __name__ == '__main__':
    rospy.init_node('listener', anonymous=True)
    rospy.Subscriber('/airsim_node/Drone1/odom_local_ned', Odometry, callback)

    #PIDcontroller()

    rospy.spin()

Does any one have any idea why this is not working?

microsoft / AirSim

weird airsim ros position data leading to interesting results when building a PID controller #2844