Page 1 of 1

ESTKALMAN: State out of bounds, resetting

Posted: Thu Nov 19, 2020 4:17 am
by PB_
Hi,
I'm trying to set up the lighthouse positioning system V2 with my crazyflie. I followed the steps here - https://www.bitcraze.io/documentation/r ... etting_up/. When I run the get_bs_geometry.py script, I get reasonable estimates, so I flash it with make clean all cload. The lighthouse deck seems to load up fine, the sequence matched the video you had posted in another thread.

Now, when I check the cfclient, the raw and compensated angles plot change as I move the drone around. But the problem is that the x,y,z state estimate plot is messed up. It does change when I move the drone around but the values are not reasonable. And in the console, I get the "ESTKALMAN: State out of bounds", resetting message.

Where could I be going wrong?

Code: Select all

SYS: Crazyflie 2.1 is up and running!
SYS: Build 77:8b22ee23e69f (2020.09 +77) MODIFIED
SYS: I am 0x203936564E4B501300270053 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: Estimator type forced
ESTIMATOR: Using Kalman (2) estimator
CONTROLLER: Using PID (1) controller
MTR-DRV: Using brushed motor driver
EEPROM: I2C connection [OK].
STORAGE: Storage check [OK].
IMU: BMI088 gyro self-test [OK]
STAB: Wait for sensor calibration...
SYS: Free heap: LHFL: Lighthouse bootloader version: 2
20936 bytes
STAB: Ready to fly.
LHFL: Firmware version 5 verified, booting deck!
Got calibration from CA34274D
ESTKALMAN: State out of bounds, resetting

Re: ESTKALMAN: State out of bounds, resetting

Posted: Thu Nov 19, 2020 9:11 am
by kimberly
Hi! This is indeed odd so let's figure it out.
  • Have you done the geometry estimation after the calibration data has been received?
  • Have you forced to use lighthouse v2 in config.mk ? (CFLAGS += -DLIGHTHOUSE_FORCE_TYPE=2
    )
  • Did you hardcode the geometry data in the firmware or did you use --write in the script?

Re: ESTKALMAN: State out of bounds, resetting

Posted: Thu Nov 19, 2020 9:55 am
by tintin
Hi,
I have the same "position estimate divergence" problem. I'm using two V1 base stations and followed the LH setup instructions, including pasting the results of the get_bs_geometry.py script into the lighhouse_core.c source file, building and flashing. The CF seems to boot without errors and even receives V1 base station calibration data (see console output below). When I display lighthouse angles via the cfclient logging/plotting function, the angles seem plausible and also react to position changes of the CF (see screenshots). However, the position estimate diverges quickly until being reset by the KF monitor.


I'm using the following flags in my build config:

CFLAGS += -DDISABLE_LIGHTHOUSE_DRIVER=0

CFLAGS += -DLIGHTHOUSE_FORCE_TYPE=1

Here's the base station data from my lighthouse_core.c:

NO_DMA_CCM_SAFE_ZERO_INIT pulseProcessor_t lighthouseCoreState = {
.bsGeometry = {
// Arena LH1
{.valid = true, .origin = {0.595701, 2.040731, 2.372068, }, .mat = {{-0.522375, 0.784552, -0.334069, }, {-0.687560, -0.619256, -0.379188, }, {-0.504367, 0.031615, 0.862910, }, }},
{.valid = true, .origin = {-2.326042, -1.218007, 2.283305, }, .mat = {{0.763096, -0.495535, 0.414886, }, {0.453586, 0.867932, 0.202370, }, {-0.460374, 0.033759, 0.887083, }, }},

// Arena LH2
//{.valid = true, .origin = {-2.057947, 0.398319, 3.109704, }, .mat = {{0.807210, 0.002766, 0.590258, }, {0.067095, 0.993078, -0.096409, }, {-0.586439, 0.117426, 0.801437, }, }},
//{.valid = true, .origin = {0.866244, -2.566829, 3.132632, }, .mat = {{-0.043296, -0.997675, -0.052627, }, {0.766284, -0.066962, 0.639003, }, {-0.641042, -0.012661, 0.767401, }, }},
},


Here's the console output:

SYS: Crazyflie 2.1 is up and running!
SYS: Build 86:8ead221731f8 (2020.09 +86) MODIFIED
SYS: I am 0x20373347344D500E001D0034 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
MTR-DRV: Using brushed motor driver
EEPROM: I2C connection [OK].
STORAGE: Storage check [OK].
IMU: BMI088 gyro self-test [OK]
STAB: Wait for sensor calibration...
SYS: Free heapLHFL: Lighthouse bootloader version: 2
: 20936 bytes
STAB: Ready to fly.
STAB: WARNING: stabilizer loop rate is off (1004)
LHFL: Firmware version 5 verified, booting deck!
Got calibration from 89D8C0D0
phase0: 0.002077
phase1: -0.014823
Got calibration from BA44770F
phase0: 0.041961
phase1: 0.021484
ESTKALMAN: State out of bounds, resetting

Re: ESTKALMAN: State out of bounds, resetting

Posted: Thu Nov 19, 2020 10:31 am
by kristoffer
Hi!
I'm not sure what the problem is, but will take a closer look.

We are currently changing a lot for the lighthouse as we are trying to reach a first stable release. We are trying to keep it fairly clean and functional during the process but it is not unlikely that we break something temporarily.

For a more stable version I would recommend the 2020.09 release. I think this commit 463110b1aae6166fdecdcfca4d3bc64532e949bc should work as well if you want more recent code. It might be worth trying one of these versions out to see if that works.

Hopefully the code base will be more stable shortly.

Re: ESTKALMAN: State out of bounds, resetting

Posted: Thu Nov 19, 2020 2:14 pm
by tintin
Hi,
thanks for the reply. Please let me know if you need more info from my setup to assist your efforts.
Regards

Re: ESTKALMAN: State out of bounds, resetting

Posted: Thu Nov 19, 2020 3:12 pm
by kimberly
If you really need to work with the latest master. I did verify as well that currently hardcoding geo info in lighthouse_core.c does cause the position to become unstable, so probably its being deleted somewhere after that.

The app layer way does still work though, as it overwrites what ever is there.

Here is a c file:

Code: Select all

#include <string.h>
#include <stdint.h>
#include <stdbool.h>

#include "app.h"

#include "FreeRTOS.h"
#include "task.h"

#include "debug.h"

#include "lighthouse_core.h"
#include "lighthouse_position_est.h"

baseStationGeometry_t sampleGeoData[1]  = {
  {.valid = true, .origin = {-1.958483,  0.542299,  3.152727, }, .mat = {{0.79721498, -0.004274, 0.60368103, }, {0.0, 0.99997503, 0.00708, }, {-0.60369599, -0.005645, 0.79719502, }, }},
  {.valid = true, .origin = {1.062398, -2.563488,  3.112367, }, .mat = {{0.018067, -0.999336, 0.031647, }, {0.76125097, 0.034269, 0.64755201, }, {-0.648206, 0.012392, 0.76136398, }, }},
};

lighthouseCalibration_t sampleCalibrationData[1] = {
  { // Base station 0
   .valid = true,
    .sweep = {
      {.tilt = -0.047058, .phase = 0.0, .curve = 0.052215, .gibphase = 2.087890, .gibmag = -0.003913, .ogeephase = 0.433105, .ogeemag = -0.049285},
      {.tilt = 0.048065, .phase = -0.005336, .curve = 0.122375, .gibphase = 2.097656, .gibmag = -0.003883, .ogeephase = 0.631835, .ogeemag = -0.034851},
    },
  },
  { // Base station 1
    .valid = true,
    .sweep = {
      {.tilt = -0.051208, .phase = 0.0, .curve = 0.011756, .gibphase = 2.136718, .gibmag = -0.006057, .ogeephase = 2.705078,},
      {.tilt = 0.045623, .phase = -0.004142, .curve = 0.104736, .gibphase = 2.349609, .gibmag = -0.003332, .ogeephase = 0.380859, .ogeemag = -0.240112,},
    },
  },
};



void appMain() {
  lighthousePositionSetGeometryData(0, &sampleGeoData[0]);
  lighthousePositionSetGeometryData(1, &sampleGeoData[1]);
  lighthouseCoreSetCalibrationData(0, &sampleCalibrationData[0]);
  lighthouseCoreSetCalibrationData(1, &sampleCalibrationData[1]);

  while(1) {
    vTaskDelay(M2T(2000));
  }
}
and the make file that comes with it

Code: Select all

APP=1
APP_STACKSIZE=300

CFLAGS += -DDISABLE_LIGHTHOUSE_DRIVER=0
CFLAGS += -DLIGHTHOUSE_FORCE_TYPE=2

VPATH += src/
PROJ_OBJ += app_main.o 
CRAZYFLIE_BASE=../..
include $(CRAZYFLIE_BASE)/Makefile
I will make an issue of this problem.

Re: ESTKALMAN: State out of bounds, resetting

Posted: Thu Nov 19, 2020 6:29 pm
by tintin
O.K., thanks for confirming the observed effect and for pointing towards the app layer option. I might try that.

Re: ESTKALMAN: State out of bounds, resetting

Posted: Fri Nov 20, 2020 7:26 am
by PB_
Hi,
Thank you all for weighing in. To answer your questions kimberly,
1. Yes, I did do the geomtery estimation after calib data was received.
2. Yes, I forced to use v2.
3. I hardcoded the geometry data
However, as kristoffer suggested, I tried the commit 463110b1aae6166fdecdcfca4d3bc64532e949bc but here i used --write in the script. And yes it worked, I was able to get a decent flight! Thanks a lot :)

BTW Can the geometry be hardcoded for this commit? I couldn't find the relevant line in the lighthouse_core.c script.

Re: ESTKALMAN: State out of bounds, resetting

Posted: Fri Nov 20, 2020 8:44 am
by kimberly
Probably in that commit the geometry still needed to be hardcoded in lighthouse_pos_est.c before we moved it to lighthouse_core.c. You can check the doc .md file related to that commit, which you can find in the doc folder in that same repository, which still should contain instructions related to that commit. Or you can go to the documentation in the 2019.09 release, which should still apply.