ESTKALMAN: State out of bounds, resetting

Topics related to the Lighthouse positioning system, configuration and use
Post Reply
PB_
Beginner
Posts: 4
Joined: Tue Nov 17, 2020 10:35 am

ESTKALMAN: State out of bounds, resetting

Post 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
Attachments
state_estimate.png
kimberly
Bitcraze
Posts: 616
Joined: Fri Jul 06, 2018 11:13 am

Re: ESTKALMAN: State out of bounds, resetting

Post 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?
tintin
Beginner
Posts: 27
Joined: Sun Feb 16, 2020 2:00 pm

Re: ESTKALMAN: State out of bounds, resetting

Post 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
Attachments
cfclient plot of divergent lh position estimate
cfclient plot of divergent lh position estimate
cfclient plot of lighthouse angles when moving the CF
cfclient plot of lighthouse angles when moving the CF
kristoffer
Bitcraze
Posts: 345
Joined: Tue Jun 30, 2015 7:47 am

Re: ESTKALMAN: State out of bounds, resetting

Post 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.
tintin
Beginner
Posts: 27
Joined: Sun Feb 16, 2020 2:00 pm

Re: ESTKALMAN: State out of bounds, resetting

Post by tintin »

Hi,
thanks for the reply. Please let me know if you need more info from my setup to assist your efforts.
Regards
kimberly
Bitcraze
Posts: 616
Joined: Fri Jul 06, 2018 11:13 am

Re: ESTKALMAN: State out of bounds, resetting

Post 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.
tintin
Beginner
Posts: 27
Joined: Sun Feb 16, 2020 2:00 pm

Re: ESTKALMAN: State out of bounds, resetting

Post by tintin »

O.K., thanks for confirming the observed effect and for pointing towards the app layer option. I might try that.
PB_
Beginner
Posts: 4
Joined: Tue Nov 17, 2020 10:35 am

Re: ESTKALMAN: State out of bounds, resetting

Post 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.
kimberly
Bitcraze
Posts: 616
Joined: Fri Jul 06, 2018 11:13 am

Re: ESTKALMAN: State out of bounds, resetting

Post 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.
Post Reply