From af6bfe38fc6d14a0560625be908a7ef9407eacf9 Mon Sep 17 00:00:00 2001 From: Annchi Li Date: Mon, 22 Feb 2021 20:09:09 -0500 Subject: [PATCH] add the overwrite by integration of state vectors --- main/main.ino | 8 ++++++++ main/workspace.hh | 1 + 2 files changed, 9 insertions(+) diff --git a/main/main.ino b/main/main.ino index e402bcc..7bb274e 100644 --- a/main/main.ino +++ b/main/main.ino @@ -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 diff --git a/main/workspace.hh b/main/workspace.hh index 0cf57a8..fb6b2b1 100644 --- a/main/workspace.hh +++ b/main/workspace.hh @@ -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