Possible Bug in ahrs:get_position for guided mode (4.7.0 beta 5)

[Michail_Belov]

I have an unexplained altitude changes in guided mode. The copter flies to a GPS point which is set using vehicle:set_target_location(Copter_target) at a rate of 1 Hz. The copter flies there, after reaching it stays stationary, but immediately and slowly looses altitude by about 15 meters in 30 seconds, than corrects partially, again very slowly. It kept its GPS position very precisely.
in lua, the altitude is set once: Copter_target:alt(INITIAL_HOVER_ALT * 100), with INITIAL HOVER ALT being 50.
after that, vehicle:set_target_location(Copter_target) is sent at a rate of 1 Hz.

After one minute, around 10:57:35 another command was issued to climb to 80 meters using the same tecnique, and again, the copter climbed, this time slightly overshooting, and then began bobbing up and down very slowly.

The behaviour is very repeatable, in all previous flights it followed the same pattern: reaching an altitude, then slowly descending by a considerable margin, then beginning to drift up and down very slowly.

In Loiter and Alt Hold mode the copter seems to stay very stable in altitude.

Flight log: www.ecoterrenos.cl/2026-06-22 10-55-07.bin

For comparison, data from a Loiter flight:

Latest Lua script: RETRANSLATOR_09.lua (35.8 KB)

Could it be that I have to use vehicle:set_target_velocity_NED(target_vel) rather then target location? That was suggested by IA, but do want to risk several hours changing the structure of the program

[Michail_Belov]

OK. I have been doing more testing, and it seems to me that there is some bug in the implementation of the guided mode altitude control or altitude source. I changed the control from vehicle:set_target_location to vehicle:set_target_velocity_NED(target_vel) , but it did not change anything as far as altitude drift is concerned.

I did two test flights
In the first test flight, there was an unsolicited variation of baro altitude from 31 to 47 (i.e. 16 meters), and the copter was doing cycles of slowly climbing and going down. The target altitude was set to 174.1 meters (which is equal to 40 meters plus home altitude). This did not change during the flight.

The copter altitude was obtained using current_pos = ahrs:get_position()/100. Once the copter climbed to that target altitude, the copter remained very close to that altitude (± 1m), Accordingly, the vz was 0 if there was a differene of less than 1 meter. current_pos altitude and 174 meters were within 1 meter. But the copter visually climbed slowly up and down several
times in a very pronounced manner, and the baro data also reflects it (as well as GPS data).
There was automatic yaw tracking by means of injecting:

if in_yaw then in_yaw:set_override(1500-math.floor(yaw_PWM_offset)) end

which would generate PWM around 1450..1550. The option to allow yaw override was set, so that the pilot (or script by injecting RC channel) can change yaw in guide mode.
The .bin file contains messages with all the parameters used for setting vz:

gcs:send_text(6, string.format( "sign:%.0f dalt:%.1f vz:%.1f dist::%.1f C:%.1f T:%.1f ",vzdirection,dalt,vz,dist,current_pos:alt()/100,Copter_target:alt()/100))
tyícal output once the copter climbed to position:

sign:1 dalt:0.4 vz:0.0 dist::0.9 C:174.5 T:174.1

In the second flight, there was unsolicited variation of baro altitude of from 24 to 34 meters (10 meters). The target altitude was set to 176.6 meters (which is equal to 40 meters plus home altitude). The whole time it stayed pretty close to 176.6 meters, varying by ± 1 meter. There are messages in the .bin file where one can check that. During this second flight, the automatic yaw tracking was disabled. Towards the end of the flight I started yawing the copter manually. Overall, the second flight had slighly lower visible variation of altitude compared to the first flight.

Below is the section responsible for setting vz, vx, and vy for the speed controller:

local current_pos = ahrs:get_position()
if current_pos then – if current_pos and vehicle:get_mode()~=9
local target_vel = Vector3f()
– if vehicle:get_mode()~=4 then vehicle:set_mode(4) end – To prevent accidental disengagement due to mode change

      local dist=current_pos:get_distance(Copter_target) 
      local bearing=current_pos:get_bearing(Copter_target) 
      local dalt=(Copter_target:alt()-current_pos:alt())/100 -- 40 - 
      local vzdirection=0
      if dalt>0 then vzdirection=-1 end
      if dalt<0 then vzdirection=1 end   --           
      
      dalt=math.abs(dalt)
      local vz=dalt/2 
      if vz > Max_Descent_Vertical_Speed then vz=Max_Descent_Vertical_Speed  end          
      vz=vzdirection*vz
      if math.abs(dalt)<1 then vz=0 end
      speed=dist/6
      if speed>Max_Descent_Horizontal_Speed then speed =Max_Descent_Horizontal_Speed end              
      if dist<1 then speed=0 end
      
      gcs:send_text(6, string.format( "sign:%.0f dalt:%.1f vz:%.1f dist::%.1f C:%.1f T:%.1f  ",vzdirection,dalt,vz,dist,current_pos:alt()/100,Copter_target:alt()/100))  

      target_vel:x(math.cos(bearing) * speed)
      target_vel:y(math.sin(bearing) * speed)
      target_vel:z(vz)
      vehicle:set_target_velocity_NED(target_vel) 
      
      -- We have: TARGET: Copter_target,  NOW:  current_pos  these are locations.
      -- Obtain distance from Target to NOW.              
      -- Obtain bearing from Target to NOW
      -- Obtain alt diff from Target to NOW
      -- Depending on the distance establish the speed (the smaller the distance, the smaller the speed)
      -- Depending on the alt diff, establish vertical speed
      -- verify that vertical speed and horizontal speed is within limits
    
         
    
    local Copter_pitch_rad = ahrs:get_pitch_rad()
    local Copter_pitch_deg = math.deg(Copter_pitch_rad)
	local distance_to_Plane = current_pos:get_distance(Plane_Pos) 
	local bearing_to_Plane_radians = current_pos:get_bearing(Plane_Pos)
	local heading_to_Plane_deg = math.deg (bearing_to_Plane_radians)
   
 	local copter_yaw=ahrs:get_yaw_rad()  -- yaw correction using injection of yaw PWM
    local copter_yawdeg=math.deg (copter_yaw)    
 	if copter_yawdeg<0 then copter_yawdeg= 360  + copter_yawdeg end      	
	local yaw_PWM_offset = 0
    local diff=copter_yawdeg- heading_to_Plane_deg
    local normalized_diff = ( diff + 180) % 360 - 180  -- normalized angle
	  
    yaw_PWM_offset=normalized_diff*6   	
    if  yaw_PWM_offset>80 then yaw_PWM_offset=70 end
    if  yaw_PWM_offset<-80 then yaw_PWM_offset=-70 end    
  	if math.abs(normalized_diff)<3 then yaw_PWM_offset=0 end
  	local in_yaw = rc:get_channel(4)  -- yaw
if in_yaw then in_yaw:set_override(1500-math.floor(yaw_PWM_offset)) end

end
end

Reworked script which was used: RETRANSLATOR_012.lua (37.8 KB)

[Michail_Belov]

In short, the ahrs:get_position() at least in guided mode returns altitude which is inconsistent with respect altitudes reported by baro and gps, and visual observation.

First flight .bin file:www.ecoterrenos.cl/FIRST.bin

Second flight .bin file:www.ecoterrenos.cl/SECOND.bin

I wounder if that could be a problem (velz is primary GPS, secondary none), should it not be BARO primary and GPS secondary?:

EK3_SRC1_POSXY	3
EK3_SRC1_POSZ	1
EK3_SRC1_VELXY	3
EK3_SRC1_VELZ	3
EK3_SRC1_YAW	1
EK3_SRC2_POSXY	0
EK3_SRC2_POSZ	1
EK3_SRC2_VELXY	0
EK3_SRC2_VELZ	0
EK3_SRC2_YAW	0

The only other possibility I see is to use gps:location():alt() and smooth out possible jumps. I know it is not a very good idea to use GPS altitude for control, but in reality, a smoothed out lagging response to GPS altitude is probably the best strategy.

[Michail_Belov]

Could someone look into this?

[Yuri_Rage]

Not likely a bug. You are comparing raw sensor output to EKF fused altitude.

[Michail_Belov]

But it is exactly my point, in my case, ahrs:position CONSISTENTLY and repeatedly returns altitude position data inferior to the raw sensors. My preliminary conclusion is that there maybe some errors in how ahrs:position processes the rawe sensors… Both baro and GPS track very closely to the visual behaviour of the copter, and yet ahrs: does not reflect at all this movement.

Unless I have EKF configuration mistake in terms of baro/gps precedence, of course.

[Yuri_Rage]

If you aren’t using a stable RTK corrected GPS source, then GPS primary could be the culprit.

I haven’t reviewed the logs - I’m not at a computer where I can do so.

[Michail_Belov]

It is not RTK, but the problem is that GPS reflected very well the visuals, i.e. the copter clearly started to descend over a tree and I was almost ready to switch to manual control modes to save it (initially I thought that I screwed the programming). But then it started climbing again, and so on. But it was very slow, a cycle was around 10..20 seconds. I am 100 % sure that the GPS was showing correct altitude, and correct changes of altitude, and the baro data shows the same.

The point is that neither GPS nor Baro changes of altitude were reflected in ahrs:position. Now, the GPS holding of horizontal position seemed to be perfect. If there was a jump in GPS altitude which did not happen in the reality (GPS drifting), I would agree with you. But here it is the other way round, the copter was drifting in altitude, and GPS was following the drift in its altitude output.

[rmackay9]

@Michail_Belov

Maybe I’m misunderstanding but it sounds like the issue may be that the script is getting the current altitude and then using that to update the target altitude thus essentially locking in any altitude error. There’s always going to be altitude error so you definitely don’t want to continuously take the actual altitude and pass it into the controllers as the target altitude. This method invariable leads to drift – large or small depends upon the EKF’s altitudes estimate quality and the vehicle’s ability to control itself.

A better approach is to only very rarely update the target. Leave it to the controllers to do their job with a solid target.

[Michail_Belov]

What the script does is to set internally a target altitude in Copter:target() global variable once.
Then, in Tracking_Plane function, the script compares that altitude with ahrs: position() altitude.

Depending on the difference, the scripty set vz and requests vxyz velocities:
target_vel:z(vz)
vehicle:set_target_velocity_NED(target_vel)

Initially, I did what you described: I was updating the vehicle:target_location continuously, but it produced that same result, and indeed it could have been a problem with overflooding the controller (that was suggested by some search results on the web).

But now, I made a crude controller which sets the speeds, not the targets.

So to put it this way: if I (the script) sets the speed vz to zero because ahrs:position() returns a fixed altitude for a long while and this altitude is the same as preset altitude, the ahrs:position does not reflect changes of the alttiude despite there being a drift in altitude both in GPS, baro, and visual

   local current_pos = ahrs:get_position()

…

  local dalt=(Copter_target:alt()-current_pos:alt())/100 
  local vzdirection=0
  if dalt>0 then vzdirection=-1 end
  if dalt<0 then vzdirection=1 end            
      
  dalt=math.abs(dalt)
  local vz=dalt/2 
  if vz > Max_Descent_Vertical_Speed then 
       vz=Max_Descent_Vertical_Speed
  end          
  vz=vzdirection*vz}

..
target_vel:x(math.cos(bearing) * speed)
target_vel:y(math.sin(bearing) * speed)
target_vel:z(vz)
vehicle:set_target_velocity_NED(target_vel)

[Michail_Belov]

To make it even more obvious: this graph shows that after around 10:10 there is GPS altitude which descends slolwy.

Yet the messages from script show that the ahrs:altitude remains stable
sign:1 dalt:0.4 vz:0.0 dist::9.2 C:174.5 T:174.1
C: is copter altitude as reported by ahrs:position
T: is target altitude set once

[rmackay9]

Hi @Michail_Belov,

I’m not sure I completely understand what’s being said but unless there’s some external information coming into the script (e.g. visual tracking of an object, etc) there shouldn’t be any need to add a controller into the script. AP’s controllers work very well and run at a much higher rate than the script can.

BTW Lua scripting has a get_target_location binding

[Michail_Belov]

I rewrote the script to include a speed controller, because I initially assumed that the controller of arducopter for some reason did not accept update rates at 10 Hz of the target.

But my speed controller behaves the same way more or less, because ahrs:position returns altitude data which is not updated/out of reality, with a lag of about 10 seconds, which causes vertical drifts of 10..20 meters.

The script which runs on a copter tracks a plane. In conceptual terms it is a moveable antenna tracker which points to a plane in yaw, points the antenna in elevation towards plane, and chases it up to a certain Max distance, and climbs to a certain Maximum altitude, and stays on the line which goes from GS to Plane. So copter stays exactly between the plane and GS

[rmackay9]

Hi @Michail_Belov,

I think there may be some significant interference on the barometer. The deviation between the barometer altitude and the EKF altitude is as high as 7m at times which is very unusual. Maybe the autopilot has been placed in a nearly air tight case or the prop wash is flowing right over the Copter? Maybe there’s a large hole below the autopilot?

[Michail_Belov]

Actually, that is a very good point, and I also noted some disrepancies between GPS and baro, although overall they track each other quite closely.

The baro is exposed, and since the arms are very long (diagonal 105 cm), and props are only 15 inch, there should not be any interaction (or so I assume) between the props and baro

[Michail_Belov]

but baro and gps overall follow each other.

[Michail_Belov]

I checked RCOU and baro jumps, and indeed they are related, i.e. when motors suddenly change power, that is when baro altitude jumps a little. No, not sure, at one point yes, at another two points no, there was no change in motor outputs

[Michail_Belov]

I think I have kind of solved it…

It could be that baro has a creep for some reason. (I tried to block out light, wind, etc., did not do much good).

However, ardupilot seems to ignore almost completely the GPS, pushing the ahrs altitude up.

I think the algorythm is wrong, because for very small altitude changes where baro could be more precise, that is the correct way, but when you see that GPS stays at a fixed altitude, but baro grows slowly, the ardupilot either must issue a warning (GPS/baro discrepancy) and/or rely more on GPS altitude as source for altitude.

My suggestion would be to set a limit for ALT difference which should generate alternative altitude estimation

[rmackay9]

Hi @Michail_Belov,

Txs for the update. So I think this issue likely covers what you’re suggesting

[Michail_Belov]

Exactly. And also to generate an automatic warning in GCS that there is a discrepancy during the flight. To discover that there is a creep took me several hours…