void calc_ellipsoid_jacob(Vector3f sample, param_t @params, ref float[] ret)
{
Vector3f offset = @params.offset;
Vector3f diag = @params.diag;
Vector3f offdiag = @params.offdiag;
Matrix3
softiron = new Matrix3(
diag.x, offdiag.x, offdiag.y,
offdiag.x, diag.y, offdiag.z,
offdiag.y, offdiag.z, diag.z
);
float A = (diag.x * (sample.x + offset.x)) + (offdiag.x * (sample.y + offset.y)) +
(offdiag.y * (sample.z + offset.z));
float B = (offdiag.x * (sample.x + offset.x)) + (diag.y * (sample.y + offset.y)) +
(offdiag.z * (sample.z + offset.z));
float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) +
(diag.z * (sample.z + offset.z));
float length = (float)(softiron * (sample + offset)).length();
// 0-2: partial derivative (offset wrt fitness fn) fn operated on sample
ret[0] = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C)) / length);
ret[1] = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C)) / length);
ret[2] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C)) / length);
// 3-5: partial derivative (diag offset wrt fitness fn) fn operated on sample
ret[3] = -1.0f * ((sample.x + offset.x) * A) / length;
ret[4] = -1.0f * ((sample.y + offset.y) * B) / length;
ret[5] = -1.0f * ((sample.z + offset.z) * C) / length;
// 6-8: partial derivative (off-diag offset wrt fitness fn) fn operated on sample
ret[6] = -1.0f * (((sample.y + offset.y) * A) + ((sample.x + offset.x) * B)) / length;
ret[7] = -1.0f * (((sample.z + offset.z) * A) + ((sample.x + offset.x) * C)) / length;
ret[8] = -1.0f * (((sample.z + offset.z) * B) + ((sample.y + offset.y) * C)) / length;
}
void calc_sphere_jacob(Vector3f sample, param_t @params, ref float[] ret)
{
Vector3f offset = @params.offset;
Vector3f diag = @params.diag;
Vector3f offdiag = @params.offdiag;
Matrix3
softiron = new Matrix3(
diag.x, offdiag.x, offdiag.y,
offdiag.x, diag.y, offdiag.z,
offdiag.y, offdiag.z, diag.z
);
float A = (diag.x * (sample.x + offset.x)) + (offdiag.x * (sample.y + offset.y)) +
(offdiag.y * (sample.z + offset.z));
float B = (offdiag.x * (sample.x + offset.x)) + (diag.y * (sample.y + offset.y)) +
(offdiag.z * (sample.z + offset.z));
float C = (offdiag.y * (sample.x + offset.x)) + (offdiag.z * (sample.y + offset.y)) +
(diag.z * (sample.z + offset.z));
float length = (float)(softiron * (sample + offset)).length();
// 0: partial derivative (radius wrt fitness fn) fn operated on sample
ret[0] = 1.0f;
// 1-3: partial derivative (offsets wrt fitness fn) fn operated on sample
ret[1] = -1.0f * (((diag.x * A) + (offdiag.x * B) + (offdiag.y * C)) / length);
ret[2] = -1.0f * (((offdiag.x * A) + (diag.y * B) + (offdiag.z * C)) / length);
ret[3] = -1.0f * (((offdiag.y * A) + (offdiag.z * B) + (diag.z * C)) / length);
}
public void ThreadProc(object data)
{
param_t param = (param_t)data;
for (int i = 0; i < 10; i++)
{
param.TheGrid.Rows[i].Cells[param.Type].Value = i;
}
}
public FillThread(DataGridView theGrid, int type)
{
_worker = new Thread(ThreadProc);
param_t param = new param_t();
param.TheGrid = theGrid;
param.Type = type;
_worker.Start(param);
}
float calc_residual(Vector3f sample, param_t @params)
{
Matrix3 softiron = new Matrix3(
@params.diag.x, @params.offdiag.x, @params.offdiag.y,
@params.offdiag.x, @params.diag.y, @params.offdiag.z,
@params.offdiag.y, @params.offdiag.z, @params.diag.z
);
return((float)(@params.radius - (softiron * (sample + @params.offset)).length()));
}
float calc_mean_squared_residuals(param_t @params)
{
if (_sample_buffer == null || _samples_collected == 0)
{
return(1.0e30f);
}
float sum = 0.0f;
for (uint16_t i = 0; i < _samples_collected; i++)
{
Vector3f sample = _sample_buffer[i].get();
float resid = calc_residual(sample, @params);
sum += sq(resid);
}
sum /= _samples_collected;
return(sum);
}
//NETLIB_CONSTRUCTOR(buffered_param_setter)
protected nld_buffered_param_setter(object owner, string name)
: base(owner, name)
{
m_sample_time = netlist_time.zero();
m_feedback = new logic_input_t(this, "FB", feedback); // clock part
m_Q = new logic_output_t(this, "Q");
m_pos = 0;
m_samples = 0;
m_param_name = new param_str_t(this, "CHAN", "");
m_param_mult = new param_fp_t(this, "MULT", 1.0);
m_param_offset = new param_fp_t(this, "OFFSET", 0.0);
m_param = null;
m_id = new param_num_t <size_t, param_num_t_operators_size_t>(this, "ID", 0);
connect("FB", "Q");
m_buffer = default;
}
/// \brief resolve parameter names to pointers
///
/// This function must be called after all device were constructed but
/// before reset is called.
public void resolve_params(netlist_time sample_time)
{
m_pos = 0;
m_sample_time = sample_time;
if (m_param_name.op() != "")
{
param_t p = state().setup().find_param(m_param_name.op()).param();
m_param = p;
if (p is param_fp_t) //if (dynamic_cast<param_fp_t *>(p) != nullptr)
{
m_param_setter = setter <param_fp_t>; //m_param_setter = setter_t(&NETLIB_NAME(buffered_param_setter)::setter<param_fp_t>, this);
}
else if (p is param_logic_t) //else if (dynamic_cast<param_logic_t *>(p) != nullptr)
{
m_param_setter = setter <param_logic_t>; //m_param_setter = setter_t(&NETLIB_NAME(buffered_param_setter)::setter<param_logic_t>, this);
}
}
}
void run_ellipsoid_fit()
{
if (_sample_buffer == null)
{
return;
}
float lma_damping = 10.0f;
float fitness = _fitness;
float fit1, fit2;
param_t fit1_params, fit2_params;
fit1_params = fit2_params = _params;
float[] JTJ = new float[COMPASS_CAL_NUM_ELLIPSOID_PARAMS * COMPASS_CAL_NUM_ELLIPSOID_PARAMS];
float[] JTJ2 = new float[COMPASS_CAL_NUM_ELLIPSOID_PARAMS * COMPASS_CAL_NUM_ELLIPSOID_PARAMS];
float[] JTFI = new float[COMPASS_CAL_NUM_ELLIPSOID_PARAMS];
// Gauss Newton Part common for all kind of extensions including LM
for (uint16_t k = 0; k < _samples_collected; k++)
{
Vector3f sample = _sample_buffer[k].get();
float[] ellipsoid_jacob = new float[COMPASS_CAL_NUM_ELLIPSOID_PARAMS];
calc_ellipsoid_jacob(sample, fit1_params, ref ellipsoid_jacob);
for (uint8_t i = 0; i < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; i++)
{
// compute JTJ
for (uint8_t j = 0; j < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; j++)
{
JTJ[i * COMPASS_CAL_NUM_ELLIPSOID_PARAMS + j] += ellipsoid_jacob[i] * ellipsoid_jacob[j];
JTJ2[i * COMPASS_CAL_NUM_ELLIPSOID_PARAMS + j] += ellipsoid_jacob[i] * ellipsoid_jacob[j];
}
// compute JTFI
JTFI[i] += ellipsoid_jacob[i] * calc_residual(sample, fit1_params);
}
}
//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
//refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
for (uint8_t i = 0; i < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; i++)
{
JTJ[i * COMPASS_CAL_NUM_ELLIPSOID_PARAMS + i] += _ellipsoid_lambda;
JTJ2[i * COMPASS_CAL_NUM_ELLIPSOID_PARAMS + i] += _ellipsoid_lambda / lma_damping;
}
if (!inverse(JTJ, JTJ, 9))
{
return;
}
if (!inverse(JTJ2, JTJ2, 9))
{
return;
}
for (uint8_t row = 0; row < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; row++)
{
for (uint8_t col = 0; col < COMPASS_CAL_NUM_ELLIPSOID_PARAMS; col++)
{
fit1_params.get_sphere_params()[row + 1] -=
JTFI[col] * JTJ[row * COMPASS_CAL_NUM_ELLIPSOID_PARAMS + col];
fit2_params.get_sphere_params()[row + 1] -=
JTFI[col] * JTJ2[row * COMPASS_CAL_NUM_ELLIPSOID_PARAMS + col];
}
}
fit1 = calc_mean_squared_residuals(fit1_params);
fit2 = calc_mean_squared_residuals(fit2_params);
if (fit1 > _fitness && fit2 > _fitness)
{
_ellipsoid_lambda *= lma_damping;
}
else if (fit2 < _fitness && fit2 < fit1)
{
_ellipsoid_lambda /= lma_damping;
fit1_params = fit2_params;
fitness = fit2;
}
else if (fit1 < _fitness)
{
fitness = fit1;
}
//--------------------Levenberg-part-ends-here--------------------------------//
if (fitness < _fitness)
{
_fitness = fitness;
_params = fit1_params;
update_completion_mask();
}
}
