Deviation issue

Firmware/software/electronics/mechanics
RyanMco
Expert
Posts: 156
Joined: Tue Apr 09, 2019 6:15 am

Deviation issue

Post by RyanMco » Mon Dec 09, 2019 6:16 pm

Hi !
I succeeded to do switch between FLOWDECK and SLAM(getting data from slam), but it's really ugly to get a big DEVIATION in state estimation(output of kalman) from FLOWDECK measurement and state estimation FROM SLAM(there're the same struct state but I mean state from flowdeck, last measurement from just FLOWDECK, and state FROM SLAM the output from kalman filter once just getting data from slam) once I switch between FLOWDECK and SLAM, the state deviation is really big between state from flowdeck, and state from slam, I just catch my crazyflie drone in my hand on height z=0.4m, the state shows in my debugger: (just using flowdeck)
state->position.x=0.3;
state->position.y=0.4;
once I still catch it in my hand at specific height z=0.4m, I switched my switch to just use data from ROS(SLAM) , I were still catch it on specific height without drifting my hand/moving my hand .. it should shows approximately the same measurement as state flowdeck measurement, but in state shows once I do a switch:
state->position.x=-0.01;
state->position.y=-0.7;

this will lead my drone to have a drift (because there's a delta between state x from flowdeck and state x from using slam while doing a switch) while hovering and doing a the switch between flowdeck/slam in his flight, so what I DID like this:
I went to function where my kalman filter get measurement from flowdeck and did like this:

Code: Select all

while (stateEstimatorHasFlowPacket(&flow) && !slam) //not using slam,slam is boolean
{ 
//if slam =0, means not using slam then enter this function because it means I use //flowdeck
kalmanCoreUpdateWithFlow(&coreData, &flow, sensors);
//I added those two rows
xLASTupdateFromFlow=state->position.x;
yLASTupdateFromFlow=state->position.y;
doneUpdate = true;
}
then those two variables that I used in this function I used this here in order to compensate on the deviation that I have in state once doing a switch between flow and slam;
so I went to the function of SLAM, and did like this:

Code: Select all

static void extPositionHandler(CRTPPacket* pk)
{
  const struct CrtpExtPosition* data = (const struct CrtpExtPosition*)pk->data;
  float xoffset, yoffset;
  xoffset=xLASTupdateFromFlow-data->x;
  yoffset=yLASTupdateFromFlow-data->x;
  ext_pos.x = data->x+xoffset;
  ext_pos.y = data->y+yoffset;
  ext_pos.z = data->z;
  ext_pos.stdDev = extPosStdDev;
  estimatorEnqueuePosition(&ext_pos);
  tickOfLastPacket = xTaskGetTickCount();
}
this function before I update it:

Code: Select all

static void extPositionHandler(CRTPPacket* pk)
{
  const struct CrtpExtPosition* data = (const struct CrtpExtPosition*)pk->data;

  ext_pos.x = data->x;
  ext_pos.y = data->y;
  ext_pos.z = data->z;
  ext_pos.stdDev = extPosStdDev;
  estimatorEnqueuePosition(&ext_pos);
  tickOfLastPacket = xTaskGetTickCount();
}
but unfortunately I still am getting a deviation in state from flowdeck measurement and slam measurement once doing switch! I want in the time to do a switch(switching between FLOWDECK measurement and SLAM MEASUREMENT)there must be a "Continuous" in state that's output of kalman, so I need my state must be Continuous while doing a switch ...

what do I do to accomplish that? my state struct (x,y estimation) once doing a switch is still not Continuous, what should I do in order to overcome on that problem?!!! thanks alot



RyanMco
Expert
Posts: 156
Joined: Tue Apr 09, 2019 6:15 am

Re: Deviation issue

Post by RyanMco » Mon Dec 09, 2019 8:37 pm

maybe I should do update with state in other place?!

kimberly
Bitcraze
Posts: 214
Joined: Fri Jul 06, 2018 11:13 am

Re: Deviation issue

Post by kimberly » Tue Dec 10, 2019 9:43 am

Hi!

So this is pretty much unknown territory for since we have not tried such a sudden switch before in the Kalman filter, so it is very difficult to predict what will happen. But to give you a direction, I can think of two approaches that might work better for you.

1. The switch is way too sudden, so make sure that the switch is more gradually than a sudden switch like you are doing now. After the Crazyflie has taken off and is hovering, increase the value of the standard deviation of the estimated noise of the flow input to the kalman filter (so that the filter will trust it less) and in the mean time, decrease the standard deviation of the estimated noise of the external position input. Hopefully this will make the filter converge te states, but you might need to experiment with how gradually the chance is, or how big the overlap the measurements should occure together.

2. Since there are new measurements entering the kalman-filter and the Flow-deck already put the state values on something different, it might be also an option to reset the kalman filter with the last state values as estimated by the flowdeck. However, this is a bit of a longshot though...

Can these ideas help you further?

RyanMco
Expert
Posts: 156
Joined: Tue Apr 09, 2019 6:15 am

Re: Deviation issue

Post by RyanMco » Tue Dec 10, 2019 4:21 pm

Hi !
I used what I already explained above, the switch is working fine, but there's something going wrong and maybe you could help me!

when I do the switch then the two led of my drone once I DO THE switch will solid on RED! and they stay on red just once I do the switch! but!!!! sometimes it wouldn't appear and my drone works fine (doing the switch fine .. ) lets say 1 from 4 times my switch passed fine, and in 3 times once I do the switch (I do switch once my drone is hovering on height 0.5m) then once doing switch(I did a switch as what I explained above) the two led of drones light red and solid on red ... any help or suggestion why it's solid on red once switching?!

just a note that my switch sometimse works fine and it's hovering after switching and the leds aren't lighting solid red !!


any help why the red solid happen once doing a switch? "somtimes it works fine once switching and there's no led with solid red"


thanks

arnaud
Bitcraze
Posts: 2275
Joined: Tue Feb 06, 2007 12:36 pm

Re: Deviation issue

Post by arnaud » Wed Dec 11, 2019 7:06 am

Are you running in debug mode and what LED are solid RED, both or just one?

If it is both RED LED and you are running debug mode, it looks like a hard-fault or an assert. This can happen for many reason. The best would be to attach a debugger to it to see why the Crazyflie has stopped. If you are not in debug mode, the watchdog should make the Crazyflie restart and the console would indicate the cause of the restart.

One thing to check first is if you are using a lot of local variable: this can make the tasks stack overflow and cause random problem. It can be fixed either by increasing the task stack or by avoiding using too many local variables.

RyanMco
Expert
Posts: 156
Joined: Tue Apr 09, 2019 6:15 am

Re: Deviation issue

Post by RyanMco » Wed Dec 11, 2019 7:30 am

Yes im using debugger (in debug mode) so how can I know once I attach my debug where my crazyflie stop/fails?
Yeah ; ur last solution maybe will solve because I use many local variables..how can overcome on that problem? How tp incrase stack task? Thanks alot!!

--two leds solid red are lighting .. Not one!
Im using also pointers .. Maybe that's problem? But not much pointers; but yeah much local variables(not pointers)

arnaud
Bitcraze
Posts: 2275
Joined: Tue Feb 06, 2007 12:36 pm

Re: Deviation issue

Post by arnaud » Wed Dec 11, 2019 8:42 am

Usually you can find out what is happening by connecting a debugger yes. You have to be careful to not reset the CPU when starting debugging though: usually I am doing that by starting gdb from the console manually since IDEs will often start by resetting the CPU.

For local variables, they should be avoided as much as possible in embedded system since the stack is usually a sparse resource. You can declare variables in the global scope or as static in a function if keeping the same value for the whole program is not a problem for your use-case (we usually do that for arrays). Another solution we have started using more and more (in the LPS and Lighthouse code for example) is to have state variables in a structure and to pass the structure pointer to all functions in the module. That way the state variable of a module can be declared globally, but there can be multiple state in the program.

Null pointer is also a common cause of hard-fault so this is also a thing to look at.

Stack size of all the tasks are declared there: https://github.com/bitcraze/crazyflie-f ... fig.h#L145. You can try increasing the stack size of the task you are working on.

RyanMco
Expert
Posts: 156
Joined: Tue Apr 09, 2019 6:15 am

Re: Deviation issue

Post by RyanMco » Wed Dec 11, 2019 1:38 pm

Hi arnoud
I didn't understand how can I by debugger know where it fails when my two red leds are lighten? Im using debugger it works fine, I put my drone in debugger (connecting it) , works fine, I run script python (that's doing hovering to my drone and it's also doing a switch) , and yeah while my drone is connecting to debugger and I run script python, once there's a switch my drone light two solid red and I'm not seeing in debugger(in eclipse) where it fails .. it's not showing that it's fails however two solid red are lighting after I run my script python and there's no notes in debugger(compiler) where fail occurs .. how to know that? in console didn't write anything while two led solid on red ...(my drone ofcourse connected /wired to my debugger)


second question, Im using pointers but it couldn't be null because what I meant by Im using pointers .. like this

Code: Select all

int flow1=1;
int *flow2=&flow1;
and then I return by a function the address flow2 to use it in other files cpp .. (to overcome on problem that I need to change variable in specific file cpp, and to use that variable with the changed value in other file cpp)
so what could be a problem that I have two leds solid on red JUST once doing a switch! and sometimes it passes and switch works fine .. really weird!

RyanMco
Expert
Posts: 156
Joined: Tue Apr 09, 2019 6:15 am

Re: Deviation issue

Post by RyanMco » Wed Dec 11, 2019 9:15 pm

In my debugger I found this in my debugger console once the two led solid on red:

Code: Select all

program received signal SIGINT, interrupt. 
0x080005b92 in xTaskGetTickCount() at src/lib/FreeRtos/task.c:1765
what's that? I don't know how to fix that interrupt (it's happen once I do a switch)

arnaud
Bitcraze
Posts: 2275
Joined: Tue Feb 06, 2007 12:36 pm

Re: Deviation issue

Post by arnaud » Thu Dec 12, 2019 6:35 am

A note about your pointer use: if you just need to use a value from different file, you can declare it in the global scope in one file (int flow) and as extern in the other files (extern int flow). This way the linker will use the first file variable everywhere you use it as extern.

As for the debugger you apparently have managed to catch the error, this is great! (it is not always easy to catch those with a debugger attached). When you see 2 red LED you can stop the execution and look at the call stack to see from what originated the error. An error in "xTaskGetTickCount" does sound very much like a stack size problem though: this function takes no argument and should not fail in normal circumstance.

Post Reply