Skip to content

Commit ac58478

Browse files
jkerpejkerpe
committed
Added units to all values
1 parent ce070e8 commit ac58478

File tree

3 files changed

+59
-53
lines changed

3 files changed

+59
-53
lines changed

‎lib/SensorFusion/src/ExtendedKalmanFilter.cpp

+13-13
Original file line numberDiff line numberDiff line change
@@ -102,10 +102,10 @@ void ExtendedKalmanFilter::init(KalmanParameter& initialParameter)
102102
void ExtendedKalmanFilter::predictionStep(const uint16_t timeStep)
103103
{
104104
/* Extract individual values into variables in favor of readability. */
105-
float positionX = m_stateVector(IDX_POSITION_X_STATE_VECTOR);
106-
float positionY = m_stateVector(IDX_POSITION_Y_STATE_VECTOR);
107-
float velocity = m_stateVector(IDX_VELOCITY_STATE_VECTOR);
108-
float orientation = m_stateVector(IDX_ORIENTATION_STATE_VECTOR);
105+
float positionX = m_stateVector(IDX_POSITION_X_STATE_VECTOR); /* In mm */
106+
float positionY = m_stateVector(IDX_POSITION_Y_STATE_VECTOR); /* In mm */
107+
float velocity = m_stateVector(IDX_VELOCITY_STATE_VECTOR); /* In mm/s */
108+
float orientation = m_stateVector(IDX_ORIENTATION_STATE_VECTOR); /* In mrad */
109109

110110
float cosOrienation = cosf(orientation / 1000.0F);
111111
float sinOrienation = sinf(orientation / 1000.0F);
@@ -165,9 +165,9 @@ IKalmanFilter::PositionData ExtendedKalmanFilter::updateStep(KalmanParameter& ka
165165

166166
/* Fill the PositionData struct */
167167
PositionData currentPosition;
168-
currentPosition.positionX = m_stateVector(IDX_POSITION_X_STATE_VECTOR);
169-
currentPosition.positionY = m_stateVector(IDX_POSITION_Y_STATE_VECTOR);
170-
currentPosition.angle = m_stateVector(IDX_ORIENTATION_STATE_VECTOR);
168+
currentPosition.positionX = m_stateVector(IDX_POSITION_X_STATE_VECTOR); /* In mm */
169+
currentPosition.positionY = m_stateVector(IDX_POSITION_Y_STATE_VECTOR); /* In mm */
170+
currentPosition.angle = m_stateVector(IDX_ORIENTATION_STATE_VECTOR); /* In mrad */
171171
return currentPosition;
172172
}
173173
/******************************************************************************
@@ -179,16 +179,16 @@ IKalmanFilter::PositionData ExtendedKalmanFilter::updateStep(KalmanParameter& ka
179179
*****************************************************************************/
180180
void ExtendedKalmanFilter::updateMeasurementVector(KalmanParameter& kalmanParameter)
181181
{
182-
m_measurementVector[IDX_POSITION_X_MEASUREMENT_VECTOR] = kalmanParameter.positionOdometryX;
183-
m_measurementVector[IDX_POSITION_Y_MEASUREMENT_VECTOR] = kalmanParameter.positionOdometryY;
184-
m_measurementVector[IDX_VELOCITY_MEASUREMENT_VECTOR] = kalmanParameter.velocityOdometry;
185-
m_measurementVector[IDX_ORIENTATION_MEASUREMENT_VECTOR] = kalmanParameter.angleOdometry;
182+
m_measurementVector[IDX_POSITION_X_MEASUREMENT_VECTOR] = kalmanParameter.positionOdometryX; /* In mm */
183+
m_measurementVector[IDX_POSITION_Y_MEASUREMENT_VECTOR] = kalmanParameter.positionOdometryY; /* In mm */
184+
m_measurementVector[IDX_VELOCITY_MEASUREMENT_VECTOR] = kalmanParameter.velocityOdometry; /* In mm/s */
185+
m_measurementVector[IDX_ORIENTATION_MEASUREMENT_VECTOR] = kalmanParameter.angleOdometry; /* In mrad */
186186
}
187187

188188
void ExtendedKalmanFilter::updateControlInputVector(KalmanParameter& kalmanParameter)
189189
{
190-
m_controlInputVector[IDX_ACCELERATION_X_CONTROL_INPUT_VECTOR] = kalmanParameter.accelerationX;
191-
m_controlInputVector[IDX_TURNRATE_CONTROL_INPUT_VECTOR] = kalmanParameter.turnRate;
190+
m_controlInputVector[IDX_ACCELERATION_X_CONTROL_INPUT_VECTOR] = kalmanParameter.accelerationX; /* In mm/s^2 */
191+
m_controlInputVector[IDX_TURNRATE_CONTROL_INPUT_VECTOR] = kalmanParameter.turnRate; /* In mrad/s */
192192
}
193193

194194
float ExtendedKalmanFilter::wrapAngle(float inputAngle)

‎lib/SensorFusion/src/ExtendedKalmanFilter.h

+16-14
Original file line numberDiff line numberDiff line change
@@ -108,7 +108,7 @@ class ExtendedKalmanFilter : public IKalmanFilter
108108
PositionData updateStep(KalmanParameter& kalmanParameter) final;
109109

110110
private:
111-
/** Estimated state vector [positionX, positionY, velocity, orientation]^T */
111+
/** Estimated state vector [positionX in mm, positionY in mm, velocity in mm/s, orientation in mrad/s]^T */
112112
Eigen::Vector<float, NUMBER_OF_STATES_N> m_stateVector;
113113

114114
/** Covariance Matrix of the state */
@@ -117,37 +117,38 @@ class ExtendedKalmanFilter : public IKalmanFilter
117117
/** Control Input Vector u=[accelerationX, accelerationY, turnRate]^T */
118118
Eigen::Vector<float, NUMBER_OF_CONTROL_INPUTS_L> m_controlInputVector;
119119

120-
/** Measurement Vector z=[positionOdometryX, positionOdometryY, orientationOdometry]^T */
120+
/** Measurement Vector z=[positionOdometryX in mm, positionOdometryY in mm, velocityOdometry in mm/s,
121+
* orientationOdometry in mrad]^T */
121122
Eigen::Vector<float, NUMBER_OF_MEASUREMENTS_M> m_measurementVector;
122123

123-
/** Index of Position in x-direction in the state vector x. */
124+
/** Index of Position in mm in x-direction in the state vector x. */
124125
static const uint8_t IDX_POSITION_X_STATE_VECTOR = 0U;
125126

126-
/** Index of Position in y-direction in the state vector x. */
127+
/** Index of Position in mm in y-direction in the state vector x. */
127128
static const uint8_t IDX_POSITION_Y_STATE_VECTOR = 1U;
128129

129-
/** Index of the velocity in the state vector x. */
130+
/** Index of the velocity in mm/s in the state vector x. */
130131
static const uint8_t IDX_VELOCITY_STATE_VECTOR = 2U;
131132

132-
/** Index of the orientation in the state vector x. */
133+
/** Index of the orientation in mrad in the state vector x. */
133134
static const uint8_t IDX_ORIENTATION_STATE_VECTOR = 3U;
134135

135-
/** Index of Position in x-direction in the measurement vector z. */
136+
/** Index of Position in mm in x-direction in the measurement vector z. */
136137
static const uint8_t IDX_POSITION_X_MEASUREMENT_VECTOR = 0U;
137138

138-
/** Index of Position in y-direction in the measurement vector z. */
139+
/** Index of Position in mm in y-direction in the measurement vector z. */
139140
static const uint8_t IDX_POSITION_Y_MEASUREMENT_VECTOR = 1U;
140141

141-
/** Index of Velocity in the measurement vector z. */
142+
/** Index of Velocity in mm/s in the measurement vector z. */
142143
static const uint8_t IDX_VELOCITY_MEASUREMENT_VECTOR = 2U;
143144

144-
/** Index of Orientation in the measurement vector z. */
145+
/** Index of Orientation in mrad in the measurement vector z. */
145146
static const uint8_t IDX_ORIENTATION_MEASUREMENT_VECTOR = 3U;
146147

147-
/** Index of Acceleration in x-direction in the control input vector u. */
148+
/** Index of Acceleration in mm/s^2 in x-direction in the control input vector u. */
148149
static const uint8_t IDX_ACCELERATION_X_CONTROL_INPUT_VECTOR = 0U;
149150

150-
/** Index of Turn Rate in the control input vector u. */
151+
/** Index of Turn Rate in mrad/s in the control input vector u. */
151152
static const uint8_t IDX_TURNRATE_CONTROL_INPUT_VECTOR = 1U;
152153

153154
/** Initial covariance matrix (notation in literature: P). */
@@ -161,15 +162,16 @@ class ExtendedKalmanFilter : public IKalmanFilter
161162

162163
/**
163164
* Writes data from a KalmanParameter Struct into the Measurement Vector as a member variable m_measurementVector
164-
* with structure: [positionOdometryX, positionOdometryY, orientationOdometry]^T
165+
* with structure:
166+
* [positionOdometryX in mm, positionOdometryY in mm, velocityOdometry in mm/s, orientationOdometry in mrad]^T
165167
*
166168
* @param[in] kalmanParameter Input Parameters for the Kalman Filter as a KalmanParameter struct.
167169
*/
168170
void updateMeasurementVector(KalmanParameter& kalmanParameter);
169171

170172
/**
171173
* Writes data from a KalmanParameter Struct into the Control Input Vector as a member variable m_controlInputVector
172-
* with structure: [accelerationX, accelerationY, turnRate]^T
174+
* with structure: [accelerationX in mm/s^2, turnRate in mrad/s]^T
173175
*
174176
* @param[in] kalmanParameter Input Parameters for the Kalman Filter as a KalmanParameter struct.
175177
*/

‎lib/SensorFusion/src/SensorFusion.cpp

+30-26
Original file line numberDiff line numberDiff line change
@@ -61,20 +61,21 @@
6161
void SensorFusion::estimateNewState(const SensorData& newSensorData)
6262
{
6363
/* Calculate the physical Values via the Sensitivity Factors. */
64-
float physicalAccelerationX =
65-
static_cast<float>(newSensorData.accelerationX) * SensorConstants::ACCELEROMETER_SENSITIVITY_FACTOR;
66-
float physicalTurnRate = static_cast<float>(newSensorData.turnRate) * SensorConstants::GYRO_SENSITIVITY_FACTOR;
64+
float physicalAccelerationX = static_cast<float>(newSensorData.accelerationX) *
65+
SensorConstants::ACCELEROMETER_SENSITIVITY_FACTOR; /* In mm/s^2 */
66+
float physicalTurnRate =
67+
static_cast<float>(newSensorData.turnRate) * SensorConstants::GYRO_SENSITIVITY_FACTOR; /* In mrad/s */
6768

6869
KalmanParameter kalmanParameter;
6970
if (true == m_isFirstIteration)
7071
{
7172
/* Directely use the Position calculated via Odometry in the first iteration */
72-
kalmanParameter.positionOdometryX = static_cast<float>(newSensorData.positionOdometryX);
73-
kalmanParameter.positionOdometryY = static_cast<float>(newSensorData.positionOdometryY);
74-
kalmanParameter.angleOdometry = static_cast<float>(newSensorData.orientationOdometry);
75-
kalmanParameter.turnRate = physicalTurnRate;
76-
kalmanParameter.accelerationX = physicalAccelerationX;
77-
kalmanParameter.velocityOdometry = 0.0F;
73+
kalmanParameter.positionOdometryX = static_cast<float>(newSensorData.positionOdometryX); /* In mm */
74+
kalmanParameter.positionOdometryY = static_cast<float>(newSensorData.positionOdometryY); /* In mm */
75+
kalmanParameter.angleOdometry = static_cast<float>(newSensorData.orientationOdometry); /* In mrad */
76+
kalmanParameter.turnRate = physicalTurnRate; /* In mrad/s */
77+
kalmanParameter.accelerationX = physicalAccelerationX; /* In mm/s^2 */
78+
kalmanParameter.velocityOdometry = 0.0F; /* In mm/s */
7879
m_kalmanFilter.init(kalmanParameter);
7980
m_estimatedPosition = {kalmanParameter.positionOdometryX, kalmanParameter.positionOdometryY,
8081
kalmanParameter.angleOdometry};
@@ -83,14 +84,17 @@ void SensorFusion::estimateNewState(const SensorData& newSensorData)
8384
else
8485
{
8586
/* Calculate the traveled euclidean Distance based upon the Odometry Values. */
86-
float deltaXOdometry = static_cast<float>(newSensorData.positionOdometryX) - m_lastOdometryPosition.positionX;
87-
float deltaYOdometry = static_cast<float>(newSensorData.positionOdometryY) - m_lastOdometryPosition.positionY;
88-
float euclideanDistance = sqrtf(powf(deltaXOdometry, 2.0F) + powf(deltaYOdometry, 2.0F));
87+
float deltaXOdometry =
88+
static_cast<float>(newSensorData.positionOdometryX) - m_lastOdometryPosition.positionX; /* In mm */
89+
float deltaYOdometry =
90+
static_cast<float>(newSensorData.positionOdometryY) - m_lastOdometryPosition.positionY; /* In mm */
91+
float euclideanDistance = sqrtf(powf(deltaXOdometry, 2.0F) + powf(deltaYOdometry, 2.0F)); /* In mm */
8992

90-
float deltaAngleOdometry = static_cast<float>(newSensorData.orientationOdometry) - m_lastOdometryPosition.angle;
91-
float updatedOrientationOdometry = m_estimatedPosition.angle + deltaAngleOdometry;
93+
float deltaAngleOdometry =
94+
static_cast<float>(newSensorData.orientationOdometry) - m_lastOdometryPosition.angle; /* In mrad */
95+
float updatedOrientationOdometry = m_estimatedPosition.angle + deltaAngleOdometry; /* In mrad */
9296

93-
float duration = static_cast<float>(newSensorData.timePeriod) / 1000.0F;
97+
float duration = static_cast<float>(newSensorData.timePeriod) / 1000.0F; /* In seconds */
9498

9599
/* Calculate the mean velocity since the last Iteration instead of using the momentary Speed.
96100
Correct the sign of the distance via the measured velocity if the velocity is negative, hence the robot drives
@@ -104,38 +108,38 @@ void SensorFusion::estimateNewState(const SensorData& newSensorData)
104108
{
105109
directionCorrection = -1.0F;
106110
}
107-
float meanVelocityOdometry = directionCorrection * euclideanDistance / duration;
111+
float meanVelocityOdometry = directionCorrection * euclideanDistance / duration; /* In mm/s */
108112

109113
/* If the robot moves slowly, the odometry might not be updated in the scope of one iteration.
110114
The velocity would be falsely assumed to be 0. In this case, the last Velocity shall be used. */
111115
if (0.0F == meanVelocityOdometry)
112116
{
113117
meanVelocityOdometry = m_lastOdometryVelocity;
114118
}
115-
m_lastOdometryVelocity = meanVelocityOdometry;
119+
m_lastOdometryVelocity = meanVelocityOdometry; /* In mm/s */
116120

117121
/* Update the measured position assuming the robot drives in the estimated Direction.
118122
Use the Correction Factor in case the robot drives Backwards. */
119123
kalmanParameter.positionOdometryX =
120124
m_estimatedPosition.positionX +
121-
directionCorrection * cosf(m_estimatedPosition.angle / 1000.0F) * euclideanDistance;
125+
directionCorrection * cosf(m_estimatedPosition.angle / 1000.0F) * euclideanDistance; /* In mm */
122126
kalmanParameter.positionOdometryY =
123127
m_estimatedPosition.positionY +
124-
directionCorrection * sinf(m_estimatedPosition.angle / 1000.0F) * euclideanDistance;
125-
kalmanParameter.angleOdometry = updatedOrientationOdometry;
126-
kalmanParameter.turnRate = physicalTurnRate;
127-
kalmanParameter.velocityOdometry = meanVelocityOdometry;
128-
kalmanParameter.accelerationX = physicalAccelerationX;
128+
directionCorrection * sinf(m_estimatedPosition.angle / 1000.0F) * euclideanDistance; /* In mm */
129+
kalmanParameter.angleOdometry = updatedOrientationOdometry; /* In mrad */
130+
kalmanParameter.turnRate = physicalTurnRate; /* In mrad/s */
131+
kalmanParameter.velocityOdometry = meanVelocityOdometry; /* In mm/s */
132+
kalmanParameter.accelerationX = physicalAccelerationX; /* In mm/s^2 */
129133

130134
/* Perform the Prediction Step and the Update Step of the Kalman Filter. */
131135
m_kalmanFilter.predictionStep(newSensorData.timePeriod);
132136
m_estimatedPosition = m_kalmanFilter.updateStep(kalmanParameter);
133137
}
134138

135139
/* Save the last Odometry values */
136-
m_lastOdometryPosition.positionX = static_cast<float>(newSensorData.positionOdometryX);
137-
m_lastOdometryPosition.positionY = static_cast<float>(newSensorData.positionOdometryY);
138-
m_lastOdometryPosition.angle = static_cast<float>(newSensorData.orientationOdometry);
140+
m_lastOdometryPosition.positionX = static_cast<float>(newSensorData.positionOdometryX); /* In mm */
141+
m_lastOdometryPosition.positionY = static_cast<float>(newSensorData.positionOdometryY); /* In mm */
142+
m_lastOdometryPosition.angle = static_cast<float>(newSensorData.orientationOdometry); /* In mm */
139143
}
140144

141145
/******************************************************************************

0 commit comments

Comments
 (0)