/**
* pick new centroids based on mean coordinates in the cluster
*/
void update_centroids()
{
uint i, a, b;
uint *dp;
uint64 d;
float k;
assignment *ap;
centroid *cp, *cp_last;
dbg("*** updating centroids...\n\n");
// initialize
for (cp = centroids, cp_last = centroids + num_clusters; cp < cp_last; cp++) {
cp->target_x = cp->sum_x / cp->num_points;
cp->target_y = cp->sum_y / cp->num_points;
cp->prev_distance = cp->best_distance;
cp->best_distance = BIG_NUM;
}
#ifdef DEBUG
for (i = 0, cp = centroids, cp_last = centroids + num_clusters; cp < cp_last; i++, cp++) {
a = datax(cp->point_id) - cp->target_x;
b = datay(cp->point_id) - cp->target_y;
dbg("old centroid %d (%d, %d), target = (%.1lf, %.1lf), distance = %d\n", i, datax(cp->point_id), datay(cp->point_id), cp->target_x, cp->target_y, a * a + b * b);
}
#endif
// go through points; for each respective centroid, find the
// point closest to the target new centroid
for (i = 0, ap = point_assignments, dp = data_points; i < num_points; i++, ap++, dp += 2) {
// get my centroid
cp = centroids + ap->centroid_id;
// calculate distance to target
a = *dp - cp->target_x;
b = *(dp + 1) - cp->target_y;
d = a * a + b * b;
// new closest point?
if (cp->best_distance > d) {
cp->best_distance = d;
cp->point_id = i;
}
}
// check improvement
improved = 0;
improvement = 0.0;
for (i = 0, cp = centroids; i < num_clusters; i++, cp++) {
dbg("new centroid %d (%d, %d), distance: %ld => %ld", i, datax(cp->point_id), datay(cp->point_id), cp->prev_distance, cp->best_distance);
if (cp->prev_distance) {
k = ((float)cp->best_distance - (float)cp->prev_distance) / (float)cp->prev_distance;
improvement += fabs(k);
improved++;
dbg(" (%.3f%%)", 100.0 * k);
}
dbg("\n");
}
dbg("\n");
dump_state();
}
int main(int argc, char **argv)
{
uint i, j, x, y;
double distance_mean, distance_sum;
float average_improvement;
assignment *ap, *ap_last;
centroid *cp;
// usage: ./kmeans /path/to/file num_points num_clusters max_iterations
input_file_name = argv[1];
num_points = atoi(argv[2]);
num_clusters = atoi(argv[3]);
max_iterations = (argc > 4 ? atoi(argv[4]) : 20);
dbg("file: %s, num_points: %d, num_clusters: %d\n\n", argv[1], num_points, num_clusters);
setup_data_points();
setup_assignments();
setup_centroids();
dump_state();
for (uint i = 0; i < max_iterations; i++) {
dbg("================ ITERATION %d ================\n\n", i);
update_assignments();
update_centroids();
if (improved) {
average_improvement = improvement / improved;
dbg("average improvement: %.1f\n\n", 100.0 * average_improvement);
// done if average improvement is less than 0.1%
if (average_improvement < 0.001)
break;
}
}
dbg("================ DONE! ================\n\n");
dump_state();
// dump final centroids, FORMAT: x, y, num points, p:c distance std dev
// calculate standard deviation of point-to-centroid distances
for (i = 0, cp = centroids; i < num_clusters; i++, cp++) {
// centroid coords
x = datax(cp->point_id);
y = datay(cp->point_id);
if (cp->num_points <= 1) {
printf("%d\t%d\t%d\t0.0\n", x, y, cp->num_points);
} else {
// calculate mean distance
distance_sum = 0.0;
for (j = 0, ap = point_assignments, ap_last = point_assignments + num_points; ap < ap_last; j++, ap++) {
if (ap->centroid_id != i) continue;
distance_sum += sqrt(ap->distance);
}
distance_mean = distance_sum / cp->num_points;
// calculate summation for sample variance
distance_sum = 0.0;
for (ap = point_assignments, ap_last = point_assignments + num_points; ap < ap_last; ap++) {
if (ap->centroid_id != i) continue;
distance_sum += pow2(sqrt(ap->distance) - distance_mean);
}
printf("%d\t%d\t%d\t%.1lf\n", x, y, cp->num_points, sqrt(distance_sum / (cp->num_points - 1)));
}
}
return 0;
}
void ThermalCalibrationHelper::calculate()
{
// baro
int count = m_baroSamples.count();
Eigen::VectorXf datax(count);
Eigen::VectorXf datay(1);
Eigen::VectorXf dataz(1);
Eigen::VectorXf datat(count);
for (int x = 0; x < count; x++) {
datax[x] = m_baroSamples[x].Pressure;
datat[x] = m_baroSamples[x].Temperature;
}
m_results.baroCalibrated = ThermalCalibration::BarometerCalibration(datax, datat, m_results.baro,
&m_results.baroInSigma, &m_results.baroOutSigma);
if (m_results.baroCalibrated) {
addInstructions(tr("Barometer is calibrated."));
} else {
qDebug() << "Failed to calibrate baro!";
addInstructions(tr("Failed to calibrate barometer!"), WizardModel::Warn);
}
m_results.baroTempMin = datat.array().minCoeff();
m_results.baroTempMax = datat.array().maxCoeff();
// gyro
count = m_gyroSamples.count();
datax.resize(count);
datay.resize(count);
dataz.resize(count);
datat.resize(count);
for (int x = 0; x < count; x++) {
datax[x] = m_gyroSamples[x].x;
datay[x] = m_gyroSamples[x].y;
dataz[x] = m_gyroSamples[x].z;
datat[x] = m_gyroSamples[x].temperature;
}
m_results.gyroCalibrated = ThermalCalibration::GyroscopeCalibration(datax, datay, dataz, datat,
m_results.gyro, m_results.gyroBias,
m_results.gyroInSigma, m_results.gyroOutSigma);
if (m_results.gyroCalibrated) {
addInstructions(tr("Gyro is calibrated."));
} else {
qDebug() << "Failed to calibrate gyro!";
addInstructions(tr("Failed to calibrate gyro!"), WizardModel::Warn);
}
// accel
m_results.accelGyroTempMin = datat.array().minCoeff();
m_results.accelGyroTempMax = datat.array().maxCoeff();
// TODO: sanity checks needs to be enforced before accel calibration can be enabled and usable.
/*
count = m_accelSamples.count();
datax.resize(count);
datay.resize(count);
dataz.resize(count);
datat.resize(count);
for(int x = 0; x < count; x++){
datax[x] = m_accelSamples[x].x;
datay[x] = m_accelSamples[x].y;
dataz[x] = m_accelSamples[x].z;
datat[x] = m_accelSamples[x].temperature;
}
m_results.accelCalibrated = ThermalCalibration::AccelerometerCalibration(datax, datay, dataz, datat, m_results.accel);
*/
m_results.accelCalibrated = false;
QString str = QStringLiteral("INFO::Calibration results") + "\n";
str += QStringLiteral("INFO::Baro cal {%1, %2, %3, %4}; initial variance: %5; Calibrated variance %6")
.arg(m_results.baro[0]).arg(m_results.baro[1]).arg(m_results.baro[2]).arg(m_results.baro[3])
.arg(m_results.baroInSigma).arg(m_results.baroOutSigma) + "\n";
str += QStringLiteral("INFO::Gyro cal x{%1, %2, %3} y{%4, %5, %6} z{%7, %8, %9}; initial variance: {%10, %11, %12}; Calibrated variance {%13, %14, %15}")
.arg(m_results.gyroBias[0]).arg(m_results.gyro[0]).arg(m_results.gyro[1])
.arg(m_results.gyroBias[1]).arg(m_results.gyro[2]).arg(m_results.gyro[3])
.arg(m_results.gyroBias[2]).arg(m_results.gyro[4]).arg(m_results.gyro[5])
.arg(m_results.gyroInSigma[0]).arg(m_results.gyroInSigma[1]).arg(m_results.gyroInSigma[2])
.arg(m_results.gyroOutSigma[0]).arg(m_results.gyroOutSigma[1]).arg(m_results.gyroOutSigma[2]) + "\n";
str += QStringLiteral("INFO::Accel cal x{%1} y{%2} z{%3}; initial variance: {%4, %5, %6}; Calibrated variance {%7, %8, %9}")
.arg(m_results.accel[0]).arg(m_results.accel[1]).arg(m_results.accel[2])
.arg(m_results.accelInSigma[0]).arg(m_results.accelInSigma[1]).arg(m_results.accelInSigma[2])
.arg(m_results.accelOutSigma[0]).arg(m_results.accelOutSigma[1]).arg(m_results.accelOutSigma[2]) + "\n";
qDebug() << str;
m_debugStream << str;
emit calculationCompleted();
closeDebugLog();
}
