altb_pmic
Testing ekf implementation for Quadro_1.
Revision 3:121647a7cddf, committed 2019-10-18
- Comitter:
- pmic
- Date:
- Fri Oct 18 21:49:59 2019 +0000
- Parent:
- 2:756446014084
- Child:
- 4:e50e18eac72b
- Commit message:
- Change Eigen.lib (just uncomment -huge). And get EKF_RP running.
Changed in this revision
| Eigen.lib | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/Eigen.lib Fri Oct 18 21:17:11 2019 +0000 +++ b/Eigen.lib Fri Oct 18 21:49:59 2019 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/users/ykuroda/code/Eigen/#13a5d365ba16 +https://os.mbed.com/users/jsoh91/code/Eigen/#3b8049da21b8
--- a/main.cpp Fri Oct 18 21:17:11 2019 +0000
+++ b/main.cpp Fri Oct 18 21:49:59 2019 +0000
@@ -18,48 +18,7 @@
Matrix<float, 8, 8> A;
Matrix<float, 8, 1> b;
-// Matrix<float, 8, 8> I;
-
-/******************************************************************************/
-
- void reset();
- float get_est_state(uint8_t i);
- void update(float gyro_x, float gyro_y, float accel_x, float accel_y);
-
- float s1;
- float c1;
- float s2;
- float c2;
-
- float g;
- float kv;
- float Ts;
- float rho;
- Matrix <float, 2, 1> var_gy;
- Matrix <float, 6, 1> var_fx;
-
- Matrix <float, 2, 1> u;
- Matrix <float, 2, 1> y;
- Matrix <float, 6, 1> x;
- Matrix <float, 6, 6> F;
- Matrix <float, 2, 6> H;
- Matrix <float, 6, 6> Q;
- Matrix <float, 2, 2> R;
- Matrix <float, 6, 6> P;
- Matrix <float, 6, 2> K;
- Matrix <float, 6, 6> I;
-
- void update_angles();
- void update_F();
- void update_H();
- void initialize_R();
- void initialize_Q();
- void update_Q();
-
- Matrix <float, 6, 1> fxd();
- Matrix <float, 2, 1> gy();
-
-/******************************************************************************/
+Matrix<float, 8, 8> I;
int main()
{
@@ -108,141 +67,20 @@
8;
// I.setIdentity();
-
-/******************************************************************************/
-
- // this->Ts = Ts;
- Ts = 0.05;
- /* [n_gyro; n_b_g; n_v] */
- var_fx << 1000, 1000, 20, 20, 10, 10;
- /* [n_acc] */
- var_gy << 40, 40;
- rho = 5000;
- kv = 0.5; /* k1/m */
- g = 9.81;
- reset();
-
-/******************************************************************************/
while(1) {
- Matrix<float, 8, 1> x = A.inverse() * b;
+ // Matrix<float, 8, 1> x = A.inverse() * b;
- // ekf_rp.update(0.0f, 0.0f, 0.0f, 0.0f);
+ ekf_rp.update(0.0f, 0.0f, 0.0f, 0.0f);
dt = timer.read();
timer.reset();
pc.printf("%i; %.6f; %.6f; %.6f; %.6f; \r\n", counter, b(0), b(1), b(2), dt);
- pc.printf("%i; %.6f; %.6f; %.6f;\r\n", counter, x(0), x(1), x(2));
+ // pc.printf("%i; %.6f; %.6f; %.6f; %.6f; \r\n", counter, b(0), b(1), b(2), dt);
counter++;
- wait_us(1000000);
+ // wait_us(1000000);
}
}
-
-void reset()
-{
- u.setZero();
- y.setZero();
- x.setZero();
- update_F();
- update_H();
- initialize_Q();
- initialize_R();
- P = Q;
- K.setZero();
- I.setIdentity();
-}
-
-void update(float gyro_x, float gyro_y, float accel_x, float accel_y)
-{
- u << gyro_x, gyro_y;
- y << accel_x, accel_y;
-
- update_F();
- // update_H(); /* H remains constant */
- update_Q();
-
- x = fxd();
- P = F * P * F.transpose() + Q;
-
- K = P * H.transpose() * ( H * P * H.transpose() + R ).inverse();
-
- x = x + K * (y - gy());
- P = (I - K * H) * P;
-}
-
-float get_est_state(uint8_t i)
-{
- // x = [ang; b_g; v]
- return x(i);
-}
-
-void update_angles()
-{
- s1 = sin(x(0));
- c1 = cos(x(0));
- s2 = sin(x(1));
- c2 = cos(x(1));
-}
-
-void update_F()
-{
- F << (Ts*c1*s2*(u(1) - x(3)))/c2 + 1, -(Ts*s1*(u(1) - x(3)))/(s2*s2 - 1), -Ts, -(Ts*s1*s2)/c2, 0, 0,
- -Ts*s1*(u(1) - x(3)), 1, 0, -Ts*c1, 0, 0,
- 0, 0, 1, 0, 0, 0,
- 0, 0, 0, 1, 0, 0,
- 0, Ts*c2*g, 0, 0, 1 - Ts*kv, 0,
- -Ts*c1*c2*g, Ts*g*s1*s2, 0, 0, 0, 1 - Ts*kv;
-}
-
-void update_H()
-{
- H << 0, 0, 0, 0, -kv, 0,
- 0, 0, 0, 0, 0, -kv;
-}
-
-void initialize_R()
-{
- R << rho*var_gy(0)/Ts, 0,
- 0, rho*var_gy(1)/Ts;
-}
-
-void initialize_Q()
-{
- Q << Ts*(var_fx(0) + (s1*s1*s2*s2*var_fx(1))/(c2*c2)), (Ts*c1*s1*s2*var_fx(1))/c2, 0, 0, 0, 0,
- (Ts*c1*s1*s2*var_fx(1))/c2, Ts*c1*c1*var_fx(1), 0, 0, 0, 0,
- 0, 0, Ts*var_fx(2), 0, 0, 0,
- 0, 0, 0, Ts*var_fx(3), 0, 0,
- 0, 0, 0, 0, Ts*var_fx(4), 0,
- 0, 0, 0, 0, 0, Ts*var_fx(5);
-}
-
-void update_Q()
-{
- Q(0,0) = Ts*(var_fx(0) + (s1*s1*s2*s2*var_fx(1))/(c2*c2));
- Q(0,1) = Ts*c1*s1*s2*var_fx(1)/c2;
- Q(1,0) = Q(0,1);
- Q(1,1) = Ts*c1*c1*var_fx(1);
-}
-
-Matrix <float, 6, 1> fxd()
-{
- Matrix <float, 6, 1> retval;
- retval << x(0) + Ts*(u(0) - x(2) + (s1*s2*(u(1) - x(3)))/c2),
- x(1) + Ts*c1*(u(1) - x(3)),
- x(2),
- x(3),
- x(4) + Ts*(g*s2 - kv*x(4)),
- x(5) - Ts*(kv*x(5) + c2*g*s1);
- return retval;
-}
-
-Matrix <float, 2, 1> gy()
-{
- Matrix <float, 2, 1> retval;
- retval << -kv*x(4),
- -kv*x(5);
- return retval;
-}