-
Notifications
You must be signed in to change notification settings - Fork 1.1k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.