Problems Kalman filter computation copy

Discussions and questions about the Crazyflie Nano Quadcopter
Post Reply
imranmomtaz
Member
Posts: 36
Joined: Tue Mar 17, 2020 7:01 pm

Problems Kalman filter computation copy

Post by imranmomtaz »

Hello,
My platform: CF2.1. I am trying to develop a dock which will essentially perform the EKF computation, but not on the original CF2.1 but on model. I essentially made a copy of the EKF codes (available from kalman_core.c). After doing this and defining the new code essentially a new deck, I am not being able to do anything. The code got compiled alright, but problem arises after flashing in the system. The quad just does not boot or anything. The M1 led is blinking. I want to ask if there is any restriction to use Axis3f variables. I defined a couple of those in the system. I am also attaching the codes with this post (the deck file is: ekfOptiEngine.c which uses some functions defined in cfJacobian.c, and its header is cfJacobian.h).

[I commented/uncommented some line of code to see the effect, and then I got the quad to fly but it looks like it is having proper sensor readings to its control computation. It is not honoring to lighthouse or flowdeck.]

I am totally lost here. Please help me how to do it proper way.

PS: Following is the console output, if this helps:

Code: Select all

SYS: ----------------------------
SYS: Crazyflie 2.1 is up and running!
SYS: Build 29:5d4350e645a2 (2020.04 +29) MODIFIED
SYS: I am 0x20383347344D5008003D002D and I have 1024KB of flash!
CFGBLK: v1, verification [OK]
DECK_INFO: DECK_FORCE=ekfOptiEngine found
DECK_INFO: compile-time forced driver ekfOptiEngine added
DECK_CORE: 3 deck(s) found
DECK_CORE: Calling INIT on driver bcLighthouse4 for deck 0
DECK_CORE: Calling INIT on driver bcFlow2 for deck 1
ZR2: Z-down sensor [OK]
PMW: Motion chip id: 0x49:0xB6
DECK_CORE: Calling INIT on driver ekfOptiEngine for deck 2
Hello Crazyflie 2.1 deck world!
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].
IMU: BMI088 gyro self-test [OK]
DECK_CORE: Deck 1 test [OK].
ekfOptiEngine test passed!
DECK_CORE: Deck 2 test [OK].
SYS: The system resumed after watchdog timeout [WARNING]
SYS: No assert information found
Attachments
cfJacobian.c
(24.96 KiB) Downloaded 114 times
ekfOptiEngine.c
(5.54 KiB) Downloaded 115 times
cfJacobian.h
(4.16 KiB) Downloaded 100 times
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: Problems Kalman filter computation copy

Post by kristoffer »

It is hard to say what the problem might be, a good approach is usually to remove everything and add it back in small pieces to figure out where it goes wrong.
I want to ask if there is any restriction to use Axis3f

Axis3f is just a type, no problem to use it.

Have you added your own deck (physical HW)? It sounds as some of your problems could be caused by problems on the expansion port.
imranmomtaz
Member
Posts: 36
Joined: Tue Mar 17, 2020 7:01 pm

Re: Problems Kalman filter computation copy

Post by imranmomtaz »

No, I have not added any physical hw. It is in software.

And how would you verify your development? Do you write your code in increments and flash it in hardware? Or is there some other way? Flashing into hardware multiple times should be inconvenient and we also need to see if the timing dependency is working properly. How to ensure that?
whoenig
Expert
Posts: 395
Joined: Mon Oct 27, 2014 2:55 am

Re: Problems Kalman filter computation copy

Post by whoenig »

Personally, I do a lot of development in different stages:

1. Conceptual design: First develop the method and a prototype in a high-level language that allows you to iterate quickly (e.g., Python or Matlab). You can ignore limitations such as real-time performance, memory limitations, etc. at this stage. Validation is done using either a dataset of collected data, or synthetically generated data.

2. C-Phase: Re-implement the core of your idea in C. Avoid any HW-specific details, so that you can compile your code on your PC. To simplify validation, I often generate bindings, so that I can re-use the same validation and plotting framework from step 1. Examples for binding generations are swig for Python and mex for Matlab. After this stage, you can be sure that your implementation is semantically correct, but perhaps the timing or other HW-specific things could cause issues.

3. Firmware phase: Add your code from step 2 to the firmware. This includes adding it to the makefile, calling your functions from step 2 with the right data (e.g., sensor measurements), and include logging variables and parameters.

4. Debugging and tuning phase: This is the stage where you potentially need frequent reflashing. However, from stage 2 you know that your code is semantically OK, so you can focus on HW-specific problems. I often start by using the usec-timer to validate the computational performance, continue with functional tests without flight, and finally use the parameters and logging framework to finetune the solution with flights.

For the last step, I really like using a Debug Adapter: When using an editor like VSCode it means that you can run the new firmware by pressing F5 and the flashing via SWD is a bit faster and does not involve touching the Crazyflie.
Post Reply