Fix the state update in the prediction step of the EKF #162
Conversation
…write previous time step while updating
| float dx = S[STATE_PX] * dt; | ||
| float dy = S[STATE_PY] * dt; | ||
| float dz = S[STATE_PZ] * dt + zacc * dt2 / 2.0f; // thrust can only be produced in the body's Z direction | ||
| dx = S[STATE_PX] * dt; |
There was a problem hiding this comment.
Why change these variables to a global scope? They are really only used for readability, as the compiler will likely optimize them out.
There was a problem hiding this comment.
There was a problem hiding this comment.
Ah I just realized with your comment that these variables aren't global, but just declared static within their scope. Regardless, this is only typically done if the variable's value needs to persist between function calls (the allocation will be .data portion of memory instead of the stack with this change) .
|
I may have gone out on a limb with the static keyword, I admit. The idea behind it was to define it on the heap instead of the stack, since it was redeclared at every loop. |
| float dz = S[STATE_PZ] * dt + acc->z * dt2 / 2.0f; // thrust can only be produced in the body's Z direction | ||
| dx = S[STATE_PX] * dt + acc->x * dt2 / 2.0f; | ||
| dy = S[STATE_PY] * dt + acc->y * dt2 / 2.0f; | ||
| dz = S[STATE_PZ] * dt + acc->z * dt2 / 2.0f; // thrust can only be produced in the body's Z direction | ||
|
|
||
| // position update | ||
| S[STATE_X] += R[0][0] * dx + R[0][1] * dy + R[0][2] * dz; |
There was a problem hiding this comment.
Note that the same bug is present in the else clause further below, and should also be updated. Good catch.
There was a problem hiding this comment.
Ah, thanks. I haven't implemented that case in my own work. I will add a commit for it.
|
Just following up on this, these changes seem good. I suggest you accept the pull request @ataffanel but remove the static keywords from the variables (on :622-:624) since there is no need for them to persist between calls and, I assume, the compiler will therefore more easily be able to optimize the variables away (?) |
|
Thanks, it is merged! @mikehamer thanks for reviewing. I agree for static, these variables will likely be optimized by the compiler and never leave the registers. |
Currently, the update overwrites the state and then updates the other states with the overwritten ones. I added a few temp variables to save the old state through the update.
I also changed some declarations pertaining to this state update to static, so the wouldn't be re-declared at 100 Hz.
The code builds in the Bitcraze VM, but I don't have access to a Crazyflie to see if it runs.