Skip to content

Commit b212aa8

Browse files
fibrfibr
committed
Estimation WIP
1 parent 5383275 commit b212aa8

12 files changed

+110
-25
lines changed

‎config/QuadEstimatorEKF.txt

+2-2
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,8 @@ InitStdDevs = .1, .1, .3, .1, .1, .3, .05
55
# Process noise model
66
QPosXYStd = .003
77
QPosZStd = .003
8-
QVelXYStd = .003
9-
QVelZStd = .009
8+
QVelXYStd = .001
9+
QVelZStd = .003
1010
QYawStd = .001
1111

1212
# GPS measurement std deviations

‎config/SimulatedSensors.txt

+2-2
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,8 @@
11
# note that sensors can not be faster than the controller rate (0.002)
22

33
[SimIMU]
4-
#AccelStd = 1, 1, 3
5-
#GyroStd = .5, .5, .5
4+
AccelStd = 1, 1, 3
5+
GyroStd = .5, .5, .5
66
dt = .002
77

88
[SimBaro]

‎src/BaseController.cpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -63,8 +63,7 @@ void BaseController::OnSensor_Range(float z)
6363
range = z;
6464
}
6565

66-
// Allows the simulator to provide perfect state data to the controller
67-
void BaseController::OverrideEstimates(V3F pos, V3F vel, Quaternion<float> attitude, V3F omega)
66+
void BaseController::UpdateEstimates(V3F pos, V3F vel, Quaternion<float> attitude, V3F omega)
6867
{
6968
estAtt = attitude;
7069
estOmega = omega;

‎src/BaseController.h

+2-3
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,6 @@ class BaseController : public DataSource
1818
BaseController(string name, string config);
1919
virtual ~BaseController() {};
2020

21-
virtual void RunEstimation() {};
2221
virtual VehicleCommand RunControl(float dt, float sim_time) { return VehicleCommand(); };
2322

2423
virtual void Init();
@@ -32,8 +31,8 @@ class BaseController : public DataSource
3231

3332
TrajectoryPoint GetNextTrajectoryPoint(float mission_time);
3433

35-
// Allows the simulator to provide perfect state data to the controller
36-
void OverrideEstimates(V3F pos, V3F vel, Quaternion<float> attitude, V3F omega);
34+
// update the vehicle state estimates the controller will use to do control
35+
virtual void UpdateEstimates(V3F pos, V3F vel, Quaternion<float> attitude, V3F omega);
3736

3837
// Access functions for graphing variables
3938
virtual bool GetData(const string& name, float& ret) const;

‎src/BaseQuadEstimator.h

+10
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,9 @@
22

33
#include "DataSource.h"
44
#include "Math/V3F.h"
5+
#include "Math/Quaternion.h"
6+
7+
using SLR::Quaternion;
58

69
class BaseQuadEstimator : public DataSource
710
{
@@ -19,5 +22,12 @@ class BaseQuadEstimator : public DataSource
1922
virtual void UpdateFromOpticalFlow(float dx, float dy) {};
2023
virtual void UpdateFromRangeSensor(float rng) {};
2124

25+
virtual void UpdateTrueError(V3F truePos, V3F trueVel, Quaternion<float> trueAtt) {};
26+
27+
virtual V3F EstimatedPosition() = 0;
28+
virtual V3F EstimatedVelocity()=0;
29+
virtual Quaternion<float> EstimatedAttitude()=0;
30+
virtual V3F EstimatedOmega()=0;
31+
2232
string _config;
2333
};

‎src/Math/Quaternion.h

+2-2
Original file line numberDiff line numberDiff line change
@@ -474,7 +474,7 @@ class Quaternion
474474
V4D b1(q1.AlignSigns(*this)._q);
475475

476476
// Compute the cosine of the angle between the two vectors.
477-
float dot = _q[0]*q1[0] + _q[1]*q1[1] + _q[2]*q1[2] + _q[3]*q1[3];
477+
float dot = b0.dot(b1);
478478
dot = CONSTRAIN(dot, -1, 1); // Robustness: Stay within domain of acos()
479479

480480
const double DOT_THRESHOLD = 0.9995;
@@ -486,7 +486,7 @@ class Quaternion
486486
}
487487

488488

489-
float theta = acos(dot); // theta_0 = angle between input vectors
489+
float theta = acos(dot); // theta = angle between input vectors
490490
float sTheta = sinf(theta);
491491

492492
float w1 = sinf((1.0f - t)*theta) / sTheta;

‎src/Math/V4D.h

+5
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,11 @@ struct V4D
6060
return V4D(v[0] - d[0], v[1] - d[1], v[2] - d[2], v[3] - d[3]);
6161
}
6262

63+
double dot(const V4D& b) const
64+
{
65+
return (v[0] * b.v[0] + v[1] * b.v[1] + v[2] * b.v[2] + v[3] * b.v[3]);
66+
}
67+
6368
double operator[](int i) const { return v[i]; }
6469
double operator()(int i) const { return v[i]; }
6570
double & operator[](int i) { return v[i]; }

‎src/QuadEstimatorEKF.cpp

+42
Original file line numberDiff line numberDiff line change
@@ -80,8 +80,30 @@ void QuadEstimatorEKF::UpdateFromIMU(V3F accel, V3F gyro)
8080

8181
// YAW -- TODO - weird that it's here..
8282
state(6) = quat.Yaw();
83+
84+
lastGyro = gyro;
85+
}
86+
87+
void QuadEstimatorEKF::UpdateTrueError(V3F truePos, V3F trueVel, Quaternion<float> trueAtt)
88+
{
89+
Vector<float, 7> trueState;
90+
trueState(0) = truePos.x;
91+
trueState(1) = truePos.y;
92+
trueState(2) = truePos.z;
93+
trueState(3) = trueVel.x;
94+
trueState(4) = trueVel.y;
95+
trueState(5) = trueVel.z;
96+
trueState(6) = trueAtt.Yaw();
97+
98+
trueError = state - trueState;
99+
if (trueError(6) > F_PI) trueError(6) -= 2.f*F_PI;
100+
if (trueError(6) < -F_PI) trueError(6) += 2.f*F_PI;
101+
102+
pitchErr = pitchEst - trueAtt.Pitch();
103+
rollErr = rollEst - trueAtt.Roll();
83104
}
84105

106+
85107
void QuadEstimatorEKF::Predict(float dt, V3F accel, V3F gyro)
86108
{
87109
Vector<float, QUAD_EKF_NUM_STATES> newState = state;
@@ -247,6 +269,16 @@ bool QuadEstimatorEKF::GetData(const string& name, float& ret) const
247269
GETTER_HELPER("Est.D.ax_g", accelG[0]);
248270
GETTER_HELPER("Est.D.ay_g", accelG[1]);
249271
GETTER_HELPER("Est.D.az_g", accelG[2]);
272+
273+
GETTER_HELPER("Est.E.x", trueError(0));
274+
GETTER_HELPER("Est.E.y", trueError(1));
275+
GETTER_HELPER("Est.E.z", trueError(2));
276+
GETTER_HELPER("Est.E.vx", trueError(3));
277+
GETTER_HELPER("Est.E.vy", trueError(4));
278+
GETTER_HELPER("Est.E.vz", trueError(5));
279+
GETTER_HELPER("Est.E.yaw", trueError(6));
280+
GETTER_HELPER("Est.E.pitch", pitchErr);
281+
GETTER_HELPER("Est.E.roll", rollErr);
250282
#undef GETTER_HELPER
251283
}
252284
return false;
@@ -274,6 +306,16 @@ vector<string> QuadEstimatorEKF::GetFields() const
274306
ret.push_back(_name + ".Est.S.vz");
275307
ret.push_back(_name + ".Est.S.yaw");
276308

309+
ret.push_back(_name + ".Est.E.x");
310+
ret.push_back(_name + ".Est.E.y");
311+
ret.push_back(_name + ".Est.E.z");
312+
ret.push_back(_name + ".Est.e.vx");
313+
ret.push_back(_name + ".Est.E.vy");
314+
ret.push_back(_name + ".Est.e.vz");
315+
ret.push_back(_name + ".Est.E.yaw");
316+
ret.push_back(_name + ".Est.E.pitch");
317+
ret.push_back(_name + ".Est.E.roll");
318+
277319
// diagnostic variables
278320
ret.push_back(_name + ".Est.D.AccelPitch");
279321
ret.push_back(_name + ".Est.D.AccelRoll");

‎src/QuadEstimatorEKF.h

+26
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,7 @@
22

33
#include "BaseQuadEstimator.h"
44
#include "matrix/math.hpp"
5+
#include "Math/Quaternion.h"
56

67
using matrix::Vector;
78
using matrix::Matrix;
@@ -35,6 +36,7 @@ class QuadEstimatorEKF : public BaseQuadEstimator
3536
float pitchEst, rollEst;
3637
float accelPitch, accelRoll; // raw pitch/roll angles as calculated from last accelerometer.. purely for graphing.
3738
V3F accelG;
39+
V3F lastGyro;
3840

3941
// generic update
4042
template<size_t numZ>
@@ -56,4 +58,28 @@ class QuadEstimatorEKF : public BaseQuadEstimator
5658
virtual vector<string> GetFields() const;
5759
string _name;
5860

61+
// error vs ground truth (trueError = estimated-actual)
62+
virtual void UpdateTrueError(V3F truePos, V3F trueVel, SLR::Quaternion<float> trueAtt);
63+
Vector<float, 7> trueError;
64+
float pitchErr, rollErr;
65+
66+
virtual V3F EstimatedPosition()
67+
{
68+
return V3F(state(0), state(1), state(2));
69+
}
70+
71+
virtual V3F EstimatedVelocity()
72+
{
73+
return V3F(state(3), state(4), state(5));
74+
}
75+
76+
virtual Quaternion<float> EstimatedAttitude()
77+
{
78+
return Quaternion<float>::FromEuler123_RPY(rollEst, pitchEst, state(6));
79+
}
80+
81+
virtual V3F EstimatedOmega()
82+
{
83+
return lastGyro;
84+
}
5985
};

‎src/Simulation/QuadDynamics.cpp

+14-6
Original file line numberDiff line numberDiff line change
@@ -101,10 +101,8 @@ int QuadDynamics::Initialize()
101101
{
102102
controller->SetTrajTimeOffset(trajTimeOffset);
103103
controller->SetTrajectoryOffset(trajOffset);
104-
if (config->Get(controlConfig + ".UseIdealEstimator", 0) == 1)
105-
{
106-
updateIdealStateCallback = MakeDelegate(controller.get(), &BaseController::OverrideEstimates);
107-
}
104+
105+
_useIdealEstimator = config->Get(controlConfig + ".UseIdealEstimator", 1);
108106
}
109107
else
110108
{
@@ -168,13 +166,23 @@ void QuadDynamics::Run(float dt, float simulationTime, int &idum, V3F externalFo
168166
{
169167
(*i)->Update(*this, estimator, controllerUpdateInterval, idum);
170168
}
169+
if (estimator)
170+
{
171+
estimator->UpdateTrueError(Position(), Velocity(), quat);
172+
}
173+
171174

172175
// This is the update of the onboard controller -- runs timeout logic, sensor filtering, estimation,
173176
// controller, and produces a new set of motor commands
174-
if (updateIdealStateCallback)
177+
if (controller && _useIdealEstimator)
178+
{
179+
controller->UpdateEstimates(Position(), Velocity(), quat, Omega());
180+
}
181+
else if(controller && estimator)
175182
{
176-
updateIdealStateCallback(Position(), Velocity(), quat, Omega());
183+
controller->UpdateEstimates(estimator->EstimatedPosition(), estimator->EstimatedVelocity(), estimator->EstimatedAttitude(), estimator->EstimatedOmega());
177184
}
185+
178186
if (controller)
179187
{
180188
curCmd = controller->RunControl(controllerUpdateInterval, simulationTime);

‎src/Simulation/QuadDynamics.h

+1-5
Original file line numberDiff line numberDiff line change
@@ -60,9 +60,6 @@ class QuadDynamics : public BaseDynamics
6060

6161
VehicleCommand curCmd;
6262

63-
FastDelegate4<V3F, V3F, Quaternion<float>, V3F> updateIdealStateCallback;
64-
65-
6663
FastDelegate1<TrajectoryPoint> followedTrajectoryCallback;
6764

6865
float GetArmLength() const { return L; }
@@ -105,7 +102,6 @@ class QuadDynamics : public BaseDynamics
105102

106103
V3F color;
107104
string _flightMode;
108-
109-
105+
bool _useIdealEstimator;
110106

111107
};

‎src/Simulation/SimulatedGPS.h

+3-3
Original file line numberDiff line numberDiff line change
@@ -14,9 +14,9 @@ class SimulatedGPS : public SimulatedQuadSensor
1414
{
1515
SimulatedQuadSensor::Init();
1616
ParamsHandle paramSys = SimpleConfig::GetInstance();
17-
_posStd = paramSys->Get(_config + ".PosStd", V3F(1, 1, 3));
18-
_posRandomWalkStd = paramSys->Get(_config + ".PosRandomWalkStd", V3F(0, 0, 0));
19-
_velStd = paramSys->Get(_config + ".VelStd", V3F(.1f, .1f, .3f));
17+
_posStd = paramSys->Get(_config + ".PosStd", V3F());
18+
_posRandomWalkStd = paramSys->Get(_config + ".PosRandomWalkStd", V3F());
19+
_velStd = paramSys->Get(_config + ".VelStd", V3F());
2020
_gpsDT = paramSys->Get(_config + ".dt", .1f);
2121

2222
_posRandomWalk = V3F();

0 commit comments

Comments
 (0)