Skip to content
Jump to bottom

add the overwrite by integration of state vectors #19

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

add the overwrite by integration of state vectors #19

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
8 changes: 8 additions & 0 deletions main/main.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -252,6 +252,14 @@ void loop()
}
else
{
ws.a_avg = {(ws.a_0[0] + ws.a_1[0])*0.5, (ws.a_0[1] + ws.a_1[1])*0.5, (ws.a_0[2] + ws.a_1[2])*0.5};
ws.r_body_in_inertial[0] += ws.dt*ws.v_body_wrt_inertial_in_inertial[0] + 0.5*a_avg[0]*ws.dt*ws.dt;
ws.r_body_in_inertial[1] += ws.dt*ws.v_body_wrt_inertial_in_inertial[1] + 0.5*a_avg[1]*ws.dt*ws.dt;
ws.r_body_in_inertial[2] += ws.dt*ws.v_body_wrt_inertial_in_inertial[2] + 0.5*a_avg[2]*ws.dt*ws.dt;

ws.v_body_wrt_inertial_in_inertial[0] += ws.dt*a_avg[0];
ws.v_body_wrt_inertial_in_inertial[1] += ws.dt*a_avg[1];
ws.v_body_wrt_inertial_in_inertial[2] += ws.dt*a_avg[2];
//construct x
//construct y
ws.x+=ws.dt*(ws.A*ws.x+ws.B*ws.u+ws.L*(ws.y-ws.C*ws.x)) //calculate next state
Expand Down
1 change: 1 addition & 0 deletions main/workspace.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,7 @@ class Workspace
// sensor measurements
float a_0[3] = {0.0, 0.0, 0.0}; // (m/s^2) linear acceleration, used for storing sensor measurements
float a_1[3] = {0.0, 0.0, 0.0}; // (m/s^2) linear acceleration, used for storing sensor measurements
float a_avg[3];
float w_0[3] = {0.0, 0.0, 0.0}; // (rad/s) angular velocity, used for storing sensor measurements
float w_1[3] = {0.0, 0.0, 0.0}; // (rad/s) angular velocity, used for storing sensor measurements

Expand Down