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;
}
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();
}
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();
}
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