void Coupled1D_ES::solve( PlasmaData* pData,
FieldData* fields,
HOMoments* moments_next_in)
{
int nx = pData->nx;
int ny = pData->ny;
int nz = pData->nz;
// Print out the solve execution
// printf("solve() is called under the LO solver");
// Solve the low order system
solver -> solve( moments_next_in, moments_old,
fields, fields_old);
// Update field data
for(int k=0; k<nz; k++)
{
for(int j=0; j<ny; j++)
{
for(int i=0; i<nx+1; i++)
{
fields->getE(i,j,k,0) = fields->getE(i,0,0,0);
fields_half->getE(i,j,k,0) = 0.5*(fields_next->getE(i,0,0,0) + fields_old->getE(i,0,0,0));
}
}
}
// Calculate the residual for Ampere equation using LO field and HO current
calc_residual( pData,
fields, fields_old,
moments_next);
}
/*
Run Gauss Newton fitting algorithm over the sample space and come up with offsets, diagonal/scale factors
and crosstalk/offdiagonal parameters
*/
void AccelCalibrator::run_fit(uint8_t max_iterations, float& fitness)
{
if (_sample_buffer == nullptr) {
return;
}
fitness = calc_mean_squared_residuals(_param.s);
float min_fitness = fitness;
union param_u fit_param = _param;
uint8_t num_iterations = 0;
while(num_iterations < max_iterations) {
float JTJ[ACCEL_CAL_MAX_NUM_PARAMS*ACCEL_CAL_MAX_NUM_PARAMS] {};
VectorP JTFI;
for(uint16_t k = 0; k<_samples_collected; k++) {
Vector3f sample;
get_sample(k, sample);
VectorN<float,ACCEL_CAL_MAX_NUM_PARAMS> jacob;
calc_jacob(sample, fit_param.s, jacob);
for(uint8_t i = 0; i < get_num_params(); i++) {
// compute JTJ
for(uint8_t j = 0; j < get_num_params(); j++) {
JTJ[i*get_num_params()+j] += jacob[i] * jacob[j];
}
// compute JTFI
JTFI[i] += jacob[i] * calc_residual(sample, fit_param.s);
}
}
if (!inverse(JTJ, JTJ, get_num_params())) {
return;
}
for(uint8_t row=0; row < get_num_params(); row++) {
for(uint8_t col=0; col < get_num_params(); col++) {
fit_param.a[row] -= JTFI[col] * JTJ[row*get_num_params()+col];
}
}
fitness = calc_mean_squared_residuals(fit_param.s);
if (isnan(fitness) || isinf(fitness)) {
return;
}
if (fitness < min_fitness) {
min_fitness = fitness;
_param = fit_param;
}
num_iterations++;
}
}
float CompassCalibrator::calc_mean_squared_residuals(const param_t& params) const
{
if(_sample_buffer == nullptr || _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;
}
float AccelCalibrator::calc_mean_squared_residuals(const struct param_t& params) const
{
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;
get_sample(i, sample);
float resid = calc_residual(sample, params);
sum += sq(resid);
}
sum /= _samples_collected;
return sum;
}
void AccelCalibrator::run_fit(uint8_t max_iterations, float& fitness)
{
if(_sample_buffer == NULL) {
return;
}
fitness = calc_mean_squared_residuals(_params);
float min_fitness = fitness;
struct param_t fit_param = _params;
float* param_array = (float*)&fit_param;
uint8_t num_iterations = 0;
while(num_iterations < max_iterations) {
float last_fitness = fitness;
float JTJ[ACCEL_CAL_MAX_NUM_PARAMS*ACCEL_CAL_MAX_NUM_PARAMS];
float JTFI[ACCEL_CAL_MAX_NUM_PARAMS];
memset(&JTJ,0,sizeof(JTJ));
memset(&JTFI,0,sizeof(JTFI));
for(uint16_t k = 0; k<_samples_collected; k++) {
Vector3f sample;
get_sample(k, sample);
float jacob[ACCEL_CAL_MAX_NUM_PARAMS];
calc_jacob(sample, fit_param, jacob);
for(uint8_t i = 0; i < get_num_params(); i++) {
// compute JTJ
for(uint8_t j = 0; j < get_num_params(); j++) {
JTJ[i*get_num_params()+j] += jacob[i] * jacob[j];
}
// compute JTFI
JTFI[i] += jacob[i] * calc_residual(sample, fit_param);
}
}
if(!inverse(JTJ, JTJ, get_num_params())) {
return;
}
for(uint8_t row=0; row < get_num_params(); row++) {
for(uint8_t col=0; col < get_num_params(); col++) {
param_array[row] -= JTFI[col] * JTJ[row*get_num_params()+col];
}
}
fitness = calc_mean_squared_residuals(fit_param);
if(isnan(fitness) || isinf(fitness)) {
return;
}
if(fitness < min_fitness) {
min_fitness = fitness;
_params = fit_param;
}
num_iterations++;
if (fitness - last_fitness < 1.0e-9f) {
break;
}
}
}
void CompassCalibrator::run_ellipsoid_fit()
{
if(_sample_buffer == nullptr) {
return;
}
const float lma_damping = 10.0f;
float fitness = _fitness;
float fit1, fit2;
param_t fit1_params, fit2_params;
fit1_params = fit2_params = _params;
float JTJ[COMPASS_CAL_NUM_ELLIPSOID_PARAMS*COMPASS_CAL_NUM_ELLIPSOID_PARAMS] = { };
float JTJ2[COMPASS_CAL_NUM_ELLIPSOID_PARAMS*COMPASS_CAL_NUM_ELLIPSOID_PARAMS] = { };
float JTFI[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[COMPASS_CAL_NUM_ELLIPSOID_PARAMS];
calc_ellipsoid_jacob(sample, fit1_params, 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_ellipsoid_params()[row] -= JTFI[col] * JTJ[row*COMPASS_CAL_NUM_ELLIPSOID_PARAMS+col];
fit2_params.get_ellipsoid_params()[row] -= 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();
}
}
