altb_pmic
Testing ekf implementation for Quadro_1.
Diff: main.cpp
- Revision:
- 1:6b803652d032
- Parent:
- 0:a0e9705be9c4
- Child:
- 2:756446014084
--- a/main.cpp Fri Oct 18 14:55:53 2019 +0000
+++ b/main.cpp Fri Oct 18 21:15:52 2019 +0000
@@ -1,4 +1,5 @@
#include "mbed.h"
+#include "iostream"
//#include "Eigen/Dense.h"
#include "Eigen/Core.h"
#include "Eigen/Geometry.h"
@@ -15,11 +16,50 @@
uint32_t counter;
-Matrix<float, 3, 3> A;
-Matrix<float, 3, 1> b;
-DiagonalMatrix<float, 3, 3> I;
+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,4,1> mat = state.block(6,0,4,1);
+ 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();
+
+/******************************************************************************/
int main()
{
@@ -29,28 +69,70 @@
counter = 0;
- A << 1, 2, 3,
- 4, 5, 6,
- 7, 8, 10;
+ /*
+ A << 1, 2, 3, 5, 1, 8,10, 1, 3,
+ 4, 5, 6, 8, 4, 2, 1, 9, 4,
+ 7, 8,10, 5, 6, 8, 4, 5, 1,
+ 4, 2,10, 8,10, 5, 6, 7, 8,
+ 1, 8, 7, 3, 4, 6, 5, 1, 7,
+ 4, 2, 7, 5, 7, 6, 9, 2, 1,
+ 5, 5, 1, 7, 4, 2, 1, 1, 9,
+ 8, 9, 7, 4, 5, 6, 1, 2, 2,
+ 1, 5, 9, 4, 8, 7, 2, 6, 3;
b << 3,
3,
- 4;
+ 4,
+ 2,
+ 7,
+ 5,
+ 1,
+ 8,
+ 1;
+ */
+
+ A << 1, 2, 3, 5, 1, 8,10, 1,
+ 4, 5, 6, 8, 4, 2, 1, 9,
+ 7, 8,10, 5, 6, 8, 4, 5,
+ 4, 2,10, 8,10, 5, 6, 7,
+ 1, 8, 7, 3, 4, 6, 5, 1,
+ 4, 2, 7, 5, 7, 6, 9, 2,
+ 5, 5, 1, 7, 4, 2, 1, 1,
+ 8, 9, 7, 4, 5, 6, 1, 2;
+ b << 3,
+ 3,
+ 4,
+ 2,
+ 7,
+ 5,
+ 1,
+ 8;
// I.setIdentity();
- I.diagonal()[0] = 1;
- I.diagonal()[1] = 1;
- I.diagonal()[2] = 1;
-
+/******************************************************************************/
+
+ // 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) {
- Vector3f x = I * A.inverse() * b;
+ Matrix<float, 8, 1> x = A.inverse() * b;
// 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), 1.0f/dt);
+ 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));
counter++;
@@ -59,3 +141,108 @@
}
}
+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;
+}