How to set the default filter be kalman filter?

Discussions related to positioning systems where the position is calculated outside the Crazyflie and sent to the device using radio, including MoCap systems
Post Reply
DarkKnight
Member
Posts: 39
Joined: Tue Jun 15, 2021 10:19 pm

How to set the default filter be kalman filter?

Post by DarkKnight »

Hello, everyone,

right now I can use the Optitrack system to receive the true pose information of crazyflie 2.1, and then I am planning to send the pose information into kalman filter(as far as I know, the default filter of crazyflie2.1 is complementary filter).

According to posts viewtopic.php?f=18&t=2831 and viewtopic.php?f=5&t=3068&p=15110&hilit= ... .mk#p15110, it seems that I have to change the config.mk. The steps is as follows:
  • go to projects/crazyflie-firmware/tools/make/, and edit config.mk. However, in this directory, I only found the config.mk.example instead of config.mk, so do I need rename config.mk.example into config.mk and then can add ESTIMATOR=kalman?
  • according to second post, then I should run 'make clean', 'make', 'make cload' in the console. I tried this, but I received the error like following picture
  • what's more, do I have to flash the edited crazyflie-firmware code into crazyflie?
  • Could I just use the function activate_kalman_estimator to enable the kalman filter instead of using config.mk?
Any comments and suggestion is helpful!
Attachments
1.png
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: How to set the default filter be kalman filter?

Post by kristoffer »

The easiest way is to set the parameter stabilizer.estimator to 2 (an example in https://github.com/bitcraze/crazyflie-l ... er.py#L250)

The difference of doing it through the config.mk file is that it will change the default value and the kalman estimator will be active after a reboot. You can read about the configuration and flashing here https://www.bitcraze.io/documentation/r ... ing/build/
DarkKnight
Member
Posts: 39
Joined: Tue Jun 15, 2021 10:19 pm

Re: How to set the default filter be kalman filter?

Post by DarkKnight »

Thank you so much for your reply.

For your mentioned way: setting the parameter stabilizer.estimator to 2, I have tried this way but I didn't get the right result. The following are steps that I followed:
  • 2.activate the Kalman filter
  • 3.rest the kalman filter
  • 4.I have used the command: scf.cf.extpos.send_extpos(1,2,3) to send an absolute position (1,2,3) , but when I log the stateEstimate.x, stateEstimate.y, stateEstimate.z and print them down, the result is(0,0,295.3391)
.

Could you help me take a look which part is not right? The test code is in the following:

Code: Select all

import logging
import time

import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.positioning.motion_commander import MotionCommander
#packages about sending external pose into estimator 
from cflib.crazyflie.extpos import Extpos

URI = 'radio://0/80/2M/E7E7E7E7E7'
DEFAULT_HEIGHT = 0.3

is_deck_attached = False


pose_est = [0, 0, 0]

def log_pose_est_callback(timestamp, data, logconf):
    #print(data)
    global pose_est

    pose_est[0] = data['stateEstimate.x']
    pose_est[1] = data['stateEstimate.y']
    pose_est[2] = data['stateEstimate.z']

    print('stateEstimate')
    print(pose_est)

def param_deck_flow(name, value_str):
    value = int(value_str)
    print(value)
    global is_deck_attached
    if value:
        is_deck_attached = True
        print('Deck is attached!')
    else:
        is_deck_attached = False
        print('Deck is NOT attached!')

def reset_estimator(cf):
    cf.param.set_value('kalman.resetEstimation', '1')
    time.sleep(0.1)
    cf.param.set_value('kalman.resetEstimation', '0')

    # time.sleep(1)
    #wait_for_position_estimator(cf)

def activate_kalman_estimator(cf):
    cf.param.set_value('stabilizer.estimator', '2')

    # Set the std deviation for the quaternion data pushed into the
    # kalman filter. The default value seems to be a bit too low.
    cf.param.set_value('locSrv.extQuatStdDev', 0.06)



if __name__ == '__main__':
    cflib.crtp.init_drivers()
    with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:

        scf.cf.param.add_update_callback(group='deck', name='bcFlow2',
                                         cb=param_deck_flow)
        time.sleep(1)
        logconf = LogConfig(name='stateEstimate', period_in_ms=10)
        logconf.add_variable('stateEstimate.x', 'float')
        logconf.add_variable('stateEstimate.y', 'float')
        logconf.add_variable('stateEstimate.z', 'float')
        scf.cf.log.add_config(logconf)
        logconf.data_received_cb.add_callback(log_pose_est_callback)
    
        #if is_deck_attached:
        logconf.start()

        activate_kalman_estimator(scf.cf)
        reset_estimator(scf.cf)

        while True:
            #scf.cf.extpos.send_extpose(1,1,1,0,0,0,1)
            scf.cf.extpos.send_extpos(1,2,3)

        logconf.stop()
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: How to set the default filter be kalman filter?

Post by kristoffer »

I think your code looks OK. I tried to run it (without any decks) and got

Code: Select all

stateEstimate
[0.9950857758522034, 2.0081491470336914, 2.9997260570526123]
stateEstimate
[0.9948585033416748, 2.0085127353668213, 2.9997098445892334]
stateEstimate
[0.9946269392967224, 2.0088863372802734, 2.9996941089630127]
Which looks pretty good to me.

I also checked in the client that the kalman filter is active and it looks good
DarkKnight
Member
Posts: 39
Joined: Tue Jun 15, 2021 10:19 pm

Re: How to set the default filter be kalman filter?

Post by DarkKnight »

Thanks. Since the code looks good, I think the problem may be caused by hardware. You mentioned that your test is without any decks, so I assume you take off both the multi-ranger and optical flow v2 decks, right?

Actually I have been stuck in here for a long time, do you know is there any other possible reason for my wrong data?

And could you teach me how to check the kalman filter is active or not in the client?
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: How to set the default filter be kalman filter?

Post by kimberly »

Hi!

Let me just jump in here!

Kristoffer means exactly that, no decks are there is no flowdeck and multiranger attached.

You can check out which values the parameters have by looking at the parameter tab. Here are some instructions how to use the parameter tab.. Kristoffer most likely first ran the script and then checked the stabilizer.estimator value right after in the cfclient (without restarting the crazyflie).

However, since you can not connect to the cfclient exactly at the same time as the script, you also are able to double check this in your script, you can setup an callback. Here in the params and logs step-by-step guide, there is one step that exactly shows how you can do this.

In the mean time, could you also check out the console tab in the CFclient and copy past the content you get if you connect to the crazyflie? Then we know on which version of the firmware you are.
DarkKnight
Member
Posts: 39
Joined: Tue Jun 15, 2021 10:19 pm

Re: How to set the default filter be kalman filter?

Post by DarkKnight »

Hey, Kimberly,

glad to hear your reply. According to your instruction, I setup an callback function to check the 'stabilizer.estimator'. The function is here,

Code: Select all

def param_stab_est_callback(name, value):
    print('The crazyflie has parameter ' + name + ' set at number: ' + value)
Also, I write the test code in my main function as

Code: Select all

        scf.cf.param.add_update_callback(group='stabilizer', name='estimator', cb=param_stab_est_callback)
        time.sleep(1)
        scf.cf.param.set_value('stabilizer.estimator', 2)
        print('pass')
        time.sleep(1)
        scf.cf.param.set_value('stabilizer.estimator', 1)
The whole test script is also listed here in case you need it to analyze:

Code: Select all

import logging
import time

import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie

from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.syncLogger import SyncLogger

from cflib.positioning.motion_commander import MotionCommander
#packages about sending external pose into estimator 
from cflib.crazyflie.extpos import Extpos

URI = 'radio://0/80/2M/E7E7E7E7E7'
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)

DEFAULT_HEIGHT = 0.3

is_deck_attached = False


pose_est = [0, 0, 0]

def log_pose_est_callback(timestamp, data, logconf):
    #print(data)
    global pose_est
    """
    pose_est[0] = data['stateEstimate.x']
    pose_est[1] = data['stateEstimate.y']
    pose_est[2] = data['stateEstimate.z']

    print('stateEstimate')
    print(pose_est)
    """

def param_deck_flow(name, value_str):
    value = int(value_str)
    print(value)
    global is_deck_attached
    if value:
        is_deck_attached = True
        print('Deck is attached!')
    else:
        is_deck_attached = False
        print('Deck is NOT attached!')

def param_stab_est_callback(name, value):
    print('The crazyflie has parameter ' + name + ' set at number: ' + value)

def reset_estimator(cf):
    cf.param.set_value('kalman.resetEstimation', 1)
    time.sleep(0.1)
    cf.param.set_value('kalman.resetEstimation', 0)

    # time.sleep(1)
    #wait_for_position_estimator(cf)

def activate_kalman_estimator(cf):
    cf.param.set_value('stabilizer.estimator', 2)

    # Set the std deviation for the quaternion data pushed into the
    # kalman filter. The default value seems to be a bit too low.
    cf.param.set_value('locSrv.extQuatStdDev', 0.06)



if __name__ == '__main__':
    cflib.crtp.init_drivers()
    with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:

        #scf.cf.param.add_update_callback(group='deck', name='bcFlow2', cb=param_deck_flow)
        
        scf.cf.param.add_update_callback(group='stabilizer', name='estimator', cb=param_stab_est_callback)
        time.sleep(1)
        scf.cf.param.set_value('stabilizer.estimator', 2)
        print('pass')
        time.sleep(1)
        scf.cf.param.set_value('stabilizer.estimator', 1)
        
        time.sleep(1)

        logconf = LogConfig(name='stateEstimate', period_in_ms=10)
        logconf.add_variable('stateEstimate.x', 'float')
        logconf.add_variable('stateEstimate.y', 'float')
        logconf.add_variable('stateEstimate.z', 'float')
        scf.cf.log.add_config(logconf)
        logconf.data_received_cb.add_callback(log_pose_est_callback)
        
        #if is_deck_attached:
        logconf.start()

        activate_kalman_estimator(scf.cf)
        reset_estimator(scf.cf)

        #scf.cf.param.add_update_callback('stabilizer', 'estimator', param_stab_est_callback)

        while True:
            #scf.cf.extpos.send_extpose(1,1,1,0,0,0,1)
            scf.cf.extpos.send_extpos(1,2,3)

        logconf.stop()
The results(see attached picture 1) shows that I can only set the 'stabilizer.estimator'=1, once I set the 'stabilizer.estimator'=2, my callback function will not have any response. I think this is the reason why my estimator doesn't work, but I don't understand why this will happen?

Another interesting test that I have done is. First of all, I take off the multi-ranger deck but I didn't take off the optical flow v2 deck. Under this case, The results(see attached picture 2) shows that I can only set the 'stabilizer.estimator'=2, once I set the 'stabilizer.estimator'=1, my callback function will not have any response. Also, the stateEstimate.x and stateEstimate.y is the data that we send, but stateEstimate.z is not(seems like the data estimated from optical flow v2 deck).

I do flash a new firmware code by using the CFclient, the console info is attached picture3. Looking forward to your reply!
Attachments
1.png
2.png
3.png
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: How to set the default filter be kalman filter?

Post by kimberly »

In the console screenprint, it seems that you are not on the latest master or the latest release of the crazyflie firmware. So let's see first if that is the cause.

Go ahead and update to the latest clean release (2021.03) via these instructions.

After that (let's do without any decks), you can change stabilizer.estimator to 2 in the parameter tab. Then to double check if this worked, check if you are getting the following in the console tab:

Code: Select all

ESTIMATOR: Using Kalman (2) estimator
Let us know if this is successful!
DarkKnight
Member
Posts: 39
Joined: Tue Jun 15, 2021 10:19 pm

Re: How to set the default filter be kalman filter?

Post by DarkKnight »

Hi, kimberly,

I have followed you instruction and update my firmware to the latest release(2021.03). Right now the filter can be set to kalman filter. However, there is one thing that is very weird, only the activate_kalman_estimator is written as follows, then the filter can be set as kalman filter correctly( about 5 seconds sleep time before and after the command "cf.param.set_value('stabilizer.estimator', '2')" is very important, without them, callback function(param_stab_est_callback) is not executed and then the filter is still the default complimentary filter).

Code: Select all

def activate_kalman_estimator(cf):
    time.sleep(5)
    cf.param.set_value('stabilizer.estimator', 2)
    time.sleep(5)

Code: Select all

def param_stab_est_callback(name, value):
    print('The crazyflie has parameter ' + name + ' set at number: ' + value)
Do you know anything about it? Thank you
jonasdn
Expert
Posts: 132
Joined: Mon Mar 01, 2021 3:13 pm

Re: How to set the default filter be kalman filter?

Post by jonasdn »

Hi DarkKnight,

In your while loop you do:

Code: Select all

while True:
            #scf.cf.extpos.send_extpose(1,1,1,0,0,0,1)
            scf.cf.extpos.send_extpos(1,2,3)
I am a bit afraid that you are starving the callback threads, could you perhaps put a sleep there somewhere?

Code: Select all

while True:
            #scf.cf.extpos.send_extpose(1,1,1,0,0,0,1)
            scf.cf.extpos.send_extpos(1,2,3)
            time.sleep(1)
And see if that improves this?
Post Reply