Skip to content

Commit 1576330

Browse files
RobertJN64RobertJN64
committed
split out the state estimator functions
1 parent 0001211 commit 1576330

File tree

4 files changed

+197
-129
lines changed

4 files changed

+197
-129
lines changed

‎lib/controller/EstimateStateFCN.cpp‎

Lines changed: 0 additions & 117 deletions
This file was deleted.

‎lib/controller/FlightEstimator.cpp‎

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
#include "matlab_funcs.h"
2+
3+
#define RTK 1
4+
5+
Matrix9_9 StateTransitionMat(Vector3 accel, Vector3 gyro, Matrix3_3 R_b2i) {
6+
// Remove angular rates from error-state. Make safety copies of all relevant
7+
// files into Archive
8+
Matrix9_9 F = Matrix9_9::Zero();
9+
F.block<3, 3>(0, 0) = -zetaCross(gyro);
10+
F.block<3, 3>(3, 6) = Matrix3_3::Identity();
11+
F.block<3, 3>(6, 0) = -R_b2i * zetaCross(accel);
12+
return F;
13+
}
14+
15+
Vector13 FlightEstimator(Vector13 x_est, constantsASTRA_t constantsASTRA, Vector15 z, float dT, Matrix9_9 &P, bool new_gps_packet) {
16+
// M-EKF Implementation
17+
// Remove bias from IMU
18+
z.segment<3>(0) = z.segment<3>(0) - x_est.segment<3>(13);
19+
z.segment<3>(3) = z.segment<3>(3) - x_est.segment<3>(10);
20+
z.segment<3>(6) = z.segment<3>(6) - x_est.segment<3>(16);
21+
22+
// Extract quaternion
23+
Vector9 dx = Vector9::Zero();
24+
Vector4 q = x_est.segment<4>(0);
25+
Vector4 qdot = 0.5 * HamiltonianProd(q) * (Vector4() << 0, z.segment<3>(3)).finished();
26+
x_est.segment<4>(0) = q + qdot * dT;
27+
q = x_est.segment<4>(0);
28+
29+
// A-priori quaternion estimate and rotation matrix
30+
q.normalize();
31+
Matrix3_3 R_b2i = quatRot(q).transpose();
32+
33+
// State Transition Matrix
34+
Matrix9_9 F = StateTransitionMat(z.segment<3>(0), z.segment<3>(3), R_b2i);
35+
36+
// Propagate rest of state using IMU
37+
x_est.segment<3>(7) = x_est.segment<3>(7) + (R_b2i * z.segment<3>(0) - (Vector3() << 0, 0, constantsASTRA.g).finished()) * dT;
38+
x_est.segment<3>(4) = x_est.segment<3>(4) + x_est.segment<3>(7) * dT;
39+
40+
// Discrete STM
41+
Matrix9_9 Phi = matrixExpPade6(F * dT);
42+
43+
// Extract Matrices
44+
Matrix9_9 Q = constantsASTRA.Q.block<9, 9>(0, 0);
45+
46+
// Process Noise Covariance and a-priori propagation step
47+
Q = 0.5 * Q;
48+
P = Phi * P * Phi.transpose() + Q;
49+
50+
// GPS Update
51+
if (new_gps_packet) {
52+
// Measurement matrix
53+
Matrix6_9 H = Matrix6_9::Zero();
54+
H.block<3, 3>(0, 3) = Matrix3_3::Identity();
55+
H.block<3, 3>(3, 6) = Matrix3_3::Identity();
56+
57+
// Measurement Covariance Matrix
58+
float gps_pos_covar = 1 * RTK + 100 * (1 - RTK);
59+
float gps_vel_covar = gps_pos_covar * 1;
60+
Matrix6_6 R = ((Vector6() << pow(gps_pos_covar, 2) * Vector3::Ones(), pow(gps_vel_covar, 2) * Vector3::Ones()).finished()).asDiagonal();
61+
62+
// A priori covariance and Kalman gain
63+
Matrix9_6 L = P * H.transpose() * (H * P * H.transpose() + R).inverse();
64+
65+
// Predicted measurements
66+
Vector6 z_hat = (Vector6() << x_est.segment<3>(4), x_est.segment<3>(7)).finished();
67+
68+
// Update Error State
69+
Matrix9_9 ILH = (Matrix9_9::Identity() - L * H);
70+
P = ILH * P * ILH.transpose() + L * R * L.transpose();
71+
Vector6 residual = (z.segment<6>(9) - z_hat);
72+
Vector9 inn = L * residual;
73+
dx = dx + inn;
74+
}
75+
76+
// Update full-state estimates
77+
Vector4 dq = (Vector4() << 1, dx.segment<3>(0) / 2).finished();
78+
dq.normalize();
79+
80+
Vector4 q_nom = HamiltonianProd(q) * dq;
81+
q_nom.normalize();
82+
x_est.segment<4>(0) = q_nom;
83+
x_est.segment<6>(4) = x_est.segment<6>(4) + dx.segment<6>(3);
84+
return x_est;
85+
}

‎lib/controller/GroundEstimator.cpp‎

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
#include "matlab_funcs.h"
2+
3+
#define RTK 1
4+
5+
Matrix18_18 StateTransitionMat(Vector3 accel, Vector3 gyro, Matrix3_3 R_b2i) {
6+
// Remove angular rates from error-state. Make safety copies of all relevant
7+
// files into Archive
8+
Matrix18_18 F = Matrix18_18::Zero();
9+
F.block<3, 3>(0, 0) = -zetaCross(gyro);
10+
F.block<3, 3>(0, 9) = -Matrix3_3::Identity();
11+
F.block<3, 3>(3, 6) = Matrix3_3::Identity();
12+
F.block<3, 3>(6, 0) = -R_b2i * zetaCross(accel);
13+
F.block<3, 3>(6, 12) = -R_b2i;
14+
return F;
15+
}
16+
17+
Vector13 GroundEstimator(Vector13 x_est, constantsASTRA_t constantsASTRA, Vector15 z, float dT, Matrix18_18 &P, bool new_imu_packet, bool new_gps_packet) {
18+
// M-EKF Implementation
19+
// Remove bias from IMU
20+
z.segment<3>(0) = z.segment<3>(0) - x_est.segment<3>(13);
21+
z.segment<3>(3) = z.segment<3>(3) - x_est.segment<3>(10);
22+
z.segment<3>(6) = z.segment<3>(6) - x_est.segment<3>(16);
23+
24+
// Extract quaternion
25+
Vector18 dx = Vector18::Zero();
26+
Vector4 q = x_est.segment<4>(0);
27+
Vector4 qdot = 0.5 * HamiltonianProd(q) * (Vector4() << 0, z.segment<3>(3)).finished();
28+
x_est.segment<4>(0) = q + qdot * dT;
29+
q = x_est.segment<4>(0);
30+
31+
// A-priori quaternion estimate and rotation matrix
32+
q.normalize();
33+
Matrix3_3 R_b2i = quatRot(q).transpose();
34+
35+
// State Transition Matrix (CHANGE!!)
36+
Matrix18_18 F = StateTransitionMat(z.segment<3>(0), z.segment<3>(3), R_b2i);
37+
38+
// Propagate rest of state using IMU
39+
x_est.segment<3>(7) = x_est.segment<3>(7) + (R_b2i * z.segment<3>(0) - (Vector3() << 0, 0, constantsASTRA.g).finished()) * dT;
40+
x_est.segment<3>(4) = x_est.segment<3>(4) + x_est.segment<3>(7) * dT;
41+
42+
// Discrete STM
43+
Matrix18_18 Phi = matrixExpPade6(F * dT);
44+
45+
// Extract Matrices
46+
Matrix18_18 Q = constantsASTRA.Q;
47+
Matrix6_6 R = constantsASTRA.R;
48+
49+
// Process Noise Covariance and a-priori propagation step
50+
Q = 0.5 * Q;
51+
P = Phi * P * Phi.transpose() + Q;
52+
53+
// IMU Update
54+
if (new_imu_packet) {
55+
// Measurement matrix
56+
Matrix6_18 H = Matrix6_18::Zero();
57+
H.block<3, 3>(0, 0) = zetaCross(R_b2i.transpose() * (Vector3() << 0, 0, constantsASTRA.g).finished());
58+
H.block<3, 3>(0, 12) = Matrix3_3::Identity();
59+
H.block<3, 3>(3, 0) = zetaCross(R_b2i.transpose() * constantsASTRA.mag);
60+
H.block<3, 3>(3, 15) = Matrix3_3::Identity();
61+
62+
// A priori covariance and Kalman gain
63+
Matrix18_6 L = P * H.transpose() * (H * P * H.transpose() + R).inverse();
64+
65+
// Predicted measurements
66+
Vector6 z_hat = (Vector6() << R_b2i.transpose() * (Vector3() << 0, 0, constantsASTRA.g).finished(), R_b2i.transpose() * constantsASTRA.mag).finished();
67+
68+
// Kalman Gain Weighting based on predicted acceleration
69+
Matrix18_18 ILH = (Matrix18_18::Identity() - L * H);
70+
P = ILH * P * ILH.transpose() + L * R * L.transpose();
71+
Vector6 residual = ((Vector6() << z.segment<3>(0), z.segment<3>(6)).finished() - z_hat);
72+
dx = dx + L * residual;
73+
}
74+
75+
// GPS Update
76+
if (new_gps_packet) {
77+
// Measurement matrix
78+
Matrix6_18 H = Matrix6_18::Zero();
79+
H.block<3, 3>(0, 3) = Matrix3_3::Identity();
80+
H.block<3, 3>(3, 6) = Matrix3_3::Identity();
81+
82+
// Measurement Covariance Matrix
83+
float gps_pos_covar = 1 * RTK + 130 * (1 - RTK);
84+
float gps_vel_covar = gps_pos_covar * 0.1;
85+
R = ((Vector6() << pow(gps_pos_covar, 2) * Vector3::Ones(), pow(gps_vel_covar, 2) * Vector3::Ones()).finished()).asDiagonal();
86+
87+
// A priori covariance and Kalman gain
88+
Matrix18_6 L = P * H.transpose() * (H * P * H.transpose() + R).inverse();
89+
90+
// Predicted measurements
91+
Vector6 z_hat = (Vector6() << x_est.segment<3>(4), x_est.segment<3>(7)).finished();
92+
93+
// Kalman Gain Weighting based on predicted acceleration
94+
Matrix18_18 ILH = (Matrix18_18::Identity() - L * H);
95+
P = ILH * P * ILH.transpose() + L * R * L.transpose();
96+
Vector6 residual = (z.segment<6>(9) - z_hat);
97+
Vector18 inn = L * residual;
98+
dx = dx + inn;
99+
}
100+
101+
// Update full-state estimates
102+
Vector4 dq = (Vector4() << 1, dx.segment<3>(0) / 2).finished();
103+
dq.normalize();
104+
105+
Vector4 q_nom = HamiltonianProd(q) * dq;
106+
q_nom.normalize();
107+
x_est.segment<4>(0) = q_nom;
108+
x_est.segment<15>(4) = x_est.segment<15>(4) + dx.segment<15>(3);
109+
x_est.segment<6>(4) = Vector6::Zero();
110+
111+
return x_est;
112+
}

‎lib/controller/matlab_helpers.cpp‎

Lines changed: 0 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -19,18 +19,6 @@ Matrix4_4 HamiltonianProd(Vector4 q) {
1919
return M;
2020
}
2121

22-
Matrix12_12 StateTransitionMat(Vector3 accel, Vector3 gyro, Matrix3_3 R_b2i) {
23-
// Remove angular rates from error-state. Make safety copies of all relevant
24-
// files into Archive
25-
Matrix12_12 F = Matrix12_12::Zero();
26-
F.block<3, 3>(0, 0) = -zetaCross(gyro);
27-
F.block<3, 3>(0, 9) = -Matrix3_3::Identity();
28-
F.block<3, 3>(3, 6) = Matrix3_3::Identity();
29-
F.block<3, 3>(6, 0) = -R_b2i * zetaCross(accel);
30-
31-
return F;
32-
}
33-
3422
Matrix12_12 matrixExpPade6(Matrix12_12 A) {
3523
using Scalar = typename Matrix12_12::Scalar;
3624
const Scalar b[] = {1.0, 0.5, 0.12, 0.0183333333333, 0.00199275362319, 0.000160590438368, 0.00000939085239292};

0 commit comments

Comments
 (0)