Page 1 of 1

Lighthouse working but no values from estimator

Posted: Tue Jun 04, 2019 3:44 am
by mousetrapper
I have a lighthouse deck reading two V1 lighthouses seemingly successfully: If I log lighthouse x/y/z position the values are highly accurate to within 10mm or so, the angles make sense and change as you would expect them to, but I usually do not see any values at all when I log stateEstimate.x/.y/.z. Sometimes the values are zero, most of the time no lines are drawn at all. I did once see sensible values for stateEstimate, but those went away again shortly after I started flying.

It looks like the data going into the estimator should be good. All the values make sense when I log them. I also added code to the estimator call in the lighthouse driver to see if position data was being enqueued or not:

Code: Select all

  
  // Make sure we feed sane data into the estimator
  if (!isfinite(ext_pos.pos[0]) || !isfinite(ext_pos.pos[1]) || !isfinite(ext_pos.pos[2])) {
    ++badCnt; // added
    return;
  }
  ext_pos.stdDev = 0.01;
  estimatorEnqueuePosition(&ext_pos);
  ++goodCnt; // added
  
When I log goodCnt and badCnt, goodCnt increases steadily and badCnt remains at or near zero, so it seems that observations are being enqueued for the estimator.

Looking at the boot log, it seems that everything starts up OK and the Kalman estimator is selected:

Code: Select all

SYS: Crazyflie 2.1 is up and running!
SYS: Build 64:24de9899cfdf (2019.02 +64) MODIFIED
SYS: I am 0x203937434848500F00340038 and I have 1024KB of flash!
CFGBLK: v1, verification [OK]
DECK_CORE: 1 deck(s) found
DECK_CORE: Calling INIT on driver bcLighthouse4 for deck 0
IMU: BMI088 Gyro I2C connection [OK].
IMU: BMI088 Accel I2C connection [OK]
IMU: BMP388 I2C connection [OK]
ESTIMATOR: Using Kalman (2) estimator
CONTROLLER: Using PID (1) controller
EEPROM: I2C connection [OK].
STAB: Wait for sensor calibration...
SYS: Free heap: 12768 bytes
LH: Lighthouse bootloader version: 2
STAB: Ready to fly.
LH: Firmware version 1 verified, booting deck!
LH: Synchronized!
Got calibration from 39FA9E7A
  phase0: 0.060455
  phase1: 0.057373
Got calibration from D959BEFB
  phase0: 0.076293
  phase1: 0.085937
Do you have any ideas about what the problem might be here? Thanks.

Re: Lighthouse working but no values from estimator

Posted: Tue Jun 04, 2019 8:52 am
by arnaud
Hi, thanks for the detailed question!

I have tested the same firmware here and for me it is working, I get angles, lighthouse raw position and a correct estimated position.

One thing that could happen is that the EKF has diverged or crashed, when no graph is shown it is often the case that the EKF outputs NaN ...

Are you starting with the Crazyflie close to x,y,z = 0,0,0? The EKF is initially at (0,0,0) and a sudden jump is one thing that can crash it.

One thing you could try, beside starting the Crazyflie not too far from the origin, is to reset the kalman filter. You can do that by setting the parameter "kalman.resetEstimation" to 1.