I am working on MAV control in GPS denied environment using px4flow and Lidar sensor.While in simulation and during experiments on real vehicle, MAV doesnt stay at its position and drifts in random direction. I am using /mavros/local_position/pose msg to provide setpoints to the MAV. Currently it just have to takeoff hover and land at the same position. What might be causing this? Here is the code for your reference.

`!/usr/bin/env python

import rospy

from geometry_msgs.msg import Point, PoseStamped

from mavrosmsgs.msg import * from mavrosmsgs.srv import *

class fcuModes: def init(self): pass

def setTakeoff(self):
    rospy.wait_for_service('mavros/cmd/takeoff')
    try:
        takeoffService = rospy.ServiceProxy('mavros/cmd/takeoff', mavros_msgs.srv.CommandTOL)
        takeoffService(altitude = 3)
    except rospy.ServiceException, e:
        print "Service takeoff call failed: %s"%e

def setArm(self):
    rospy.wait_for_service('mavros/cmd/arming')
    try:
        armService = rospy.ServiceProxy('mavros/cmd/arming', mavros_msgs.srv.CommandBool)
        armService(True)
    except rospy.ServiceException, e:
        print "Service arming call failed: %s"%e

def setDisarm(self):
    rospy.wait_for_service('mavros/cmd/arming')
    try:
        armService = rospy.ServiceProxy('mavros/cmd/arming', mavros_msgs.srv.CommandBool)
        armService(False)
    except rospy.ServiceException, e:
        print "Service disarming call failed: %s"%e

def setStabilizedMode(self):
    rospy.wait_for_service('mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
        flightModeService(custom_mode='STABILIZED')
    except rospy.ServiceException, e:
        print "service set_mode call failed: %s. Stabilized Mode could not be set."%e

def setOffboardMode(self):
    rospy.wait_for_service('mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
        flightModeService(custom_mode='OFFBOARD')
    except rospy.ServiceException, e:
        print "service set_mode call failed: %s. Offboard Mode could not be set."%e

def setAltitudeMode(self):
    rospy.wait_for_service('mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
        flightModeService(custom_mode='ALTCTL')
    except rospy.ServiceException, e:
        print "service set_mode call failed: %s. Altitude Mode could not be set."%e

def setPositionMode(self):
    rospy.wait_for_service('mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
        flightModeService(custom_mode='POSCTL')
    except rospy.ServiceException, e:
        print "service set_mode call failed: %s. Position Mode could not be set."%e

def setAutoLandMode(self):
    rospy.wait_for_service('mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
        flightModeService(custom_mode='AUTO.LAND')
    except rospy.ServiceException, e:
           print "service set_mode call failed: %s. Autoland Mode could not be set."%e

class Controller:

def __init__(self):
    # Drone state
    self.state = State()
    # Instantiate a setpoints message
    self.sp = PositionTarget()
    # set the flag to use position setpoints and yaw angle
    self.sp.type_mask = int('010111111000', 2)
    # LOCAL_NED
    self.sp.coordinate_frame = 1

    # We will fly at a fixed altitude for now
    # Altitude setpoint, [meters]
    self.ALT_SP = 1.5
    # update the setpoint message with the required altitude
    self.sp.position.z = self.ALT_SP
    # Step size for position update
    self.STEP_SIZE = 2.0
    # Fence. We will assume a square fence for now
    self.FENCE_LIMIT = 5.0

    # A Message for the current local position of the drone
    self.local_pos = Point(0.0, 0.0, 1.0)

    # initial values for setpoints
    self.sp.position.x = 0.0
    self.sp.position.y = 0.0

    # speed of the drone is set using MPC_XY_CRUISE parameter in MAVLink
    # using QGroundControl. By default it is 5 m/s.

# Callbacks

## local position callback
def posCb(self, msg):
    self.local_pos.x = msg.pose.position.x
    self.local_pos.y = msg.pose.position.y
    self.local_pos.z = msg.pose.position.z



## Drone State callback
def stateCb(self, msg):
    self.state = msg

## Update setpoint message
def updateSp(self):
    self.sp.position.x = self.local_pos.x
    self.sp.position.y = self.local_pos.y

def x_dir(self):
    self.sp.position.x = self.local_pos.x + 5
    self.sp.position.y = self.local_pos.y

def neg_x_dir(self):
    self.sp.position.x = self.local_pos.x - 5
    self.sp.position.y = self.local_pos.y

def y_dir(self):
    self.sp.position.x = self.local_pos.x
    self.sp.position.y = self.local_pos.y + 5

def neg_y_dir(self):
    self.sp.position.x = self.local_pos.x
    self.sp.position.y = self.local_pos.y - 5

def main():

rospy.init_node('pos_ctrl', anonymous=True)

# flight mode object
modes = fcuModes()


cnt = Controller()


rate = rospy.Rate(20.0)


rospy.Subscriber('mavros/state', State, cnt.stateCb)

# Subscribe to drone's local position
rospy.Subscriber('mavros/local_position/pose', PoseStamped, cnt.posCb)



# Setpoint publisher    
sp_pub = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=1)


# Make sure the drone is armed
while not cnt.state.armed:
    modes.setArm()
    rate.sleep()

# set in takeoff mode and takeoff to default altitude (3 m)
# modes.setTakeoff()
# rate.sleep()

# We need to send few setpoint messages, then activate OFFBOARD mode, to take effect
k=0
while k<10:
    sp_pub.publish(cnt.sp)
    rate.sleep()
    k = k + 1

# activate OFFBOARD mode
modes.setOffboardMode()

# ROS main loop
while not rospy.is_shutdown():
x=0
while x<500:
     rate.sleep()
          cnt.updateSp()
         sp_pub.publish(cnt.sp)
     rate.sleep()
     x= x + 1




modes.setAutoLandMode()

if name == 'main': try: main() except rospy.ROSInterruptException: pass`

def posCb(self, msg):
    self.local_pos.x = msg.pose.position.x

def updateSp(self):
    self.sp.position.x = self.local_pos.x