Da der OP wohl nicht mehr interessiert ist, möchte ich hier meine Daten doch noch vorstellen.
Erstmal ein Bild :)
raw = rohe unkalibrierte Daten. habe hier extra ein sehr verzogenes System erstellen lassen.
Winkel X-Achse/Y-Achse: 70°
Winkel Z-Achse/Y-Achse: 110°
Also Extrem verstellt.
Grafische Darstellung:
26500
Standardabweichung und Mittelwert der Länge der "g" Vektoren:
26501
Wie man an dem Mittelwert und der Standardabweichung der transformierten (kalibrierten) Werten sieht, ist die Länge "g" nach der Kalibrierung wirklich gut.
0: Was ist das Ziel?
Es soll ein Weg gefunden werden um einen Beschleunigungssensor exakt zu kalibrieren.
1: Wie wurden die Samplewerte erstellt?
Das Model ist eine Verkettung von Matrixen:
BIAS: Verstellung am Ursprung, ein Fehlerhafter Offset
AD: Analog Digitalwandlung
BV: Verstellung der Basisvektoren, das schief stellen der Achsen(Sensoren) zueinander.
Welt zu Sampledaten
W2S: AD . BV . BIAS
S2W: [ s1x s2x s3x ]
[ 0 s2y s3y ]
[ 0 0 s3z ]
Diese Matrix wird also gesucht.
Damit habe ich per Zufallsgenerator und Rotation eines "g" Vektors um die 3 Achsen meine Samplewerte erstellt.
Das Ziel des gesamten Algorithmus ist also die inverse Matrix S2W.
2: Wie gehts von den Samplewerten zu der Umkehrmatrix S2W?
Leider konnte ich nicht per Ableitung nach den 7 Parametern eine Lösung finden, deshalb habe ich ein numerisches Lösungsverfahren angewandt. Simulierte Abkühlung wie ohen geschrieben.
Hier ist mein Code:
/* Copyright (C) 2013 Georg Gast
schorsch_76@gmx.de
This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA
*/
#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <random>
#include <array>
#include <vector>
#include <string>
#include <limits>
#include <thread>
#include <functional>
// ------------------------------------------------
// base class
// ------------------------------------------------
template <class param_type, size_t number_parameter>
class SimualtedAnneal
{
public:
typedef std::array<param_type,number_parameter> ArrayT;
SimualtedAnneal()
: m_distribution_01(0,1)
{
}
virtual ~SimualtedAnneal() {}
ArrayT Anneal()
{
m_min_e = std::numeric_limits<param_type>::max();
size_t k;
param_type temperature, e, enew, ebest, p, r;
ArrayT snew;
// initial state
ArrayT s = s0();
ArrayT sbest = s;
e = ebest = E(s);
k = 0;
while (k < GetMaxCycles() && e > MaxAllowedE())
{
// current temperature
temperature = StartTemperature() * (param_type(1) - param_type(k) / GetMaxCycles());
snew = Neighbor(s);
enew = E(snew);
// Should we move on?
p = P(e,enew,temperature);
r = m_distribution_01(m_rnd);
if (p > r)
{
// yes, change state
e = enew;
s = snew;
}
// we found a better solution?
if (enew < ebest)
{
// this is our new best solution
ebest = enew;
sbest = snew;
m_min_e = ebest;
}
// next iteration
++k;
}
return sbest;
}
virtual param_type Check(ArrayT params)
{
return E(params);
}
protected:
// temperature
virtual param_type StartTemperature() = 0;
// iteration
virtual param_type MaxAllowedE() = 0;
virtual size_t GetMaxCycles() = 0;
// Neighbor
virtual ArrayT s0() = 0;
virtual ArrayT Neighbor(ArrayT s) = 0;
// propability to change the state
// higher temperature --> more propable
// higher difference --> more propable
// can be overridden, if needed
virtual param_type P(param_type e, param_type enew, param_type temperature)
{
param_type delta_e = enew-e;
return std::exp(-delta_e / temperature);
}
// get the current energy
virtual param_type E(const ArrayT& params) = 0;
private:
// random generators
std::random_device m_rnd;
std::uniform_real_distribution<param_type> m_distribution_01;
param_type m_min_e;
};
// ------------------------------------------------
// GyroSA
// ------------------------------------------------
template <class param_type = double>
class GyroSA : public SimualtedAnneal<param_type,6>
{
public:
typedef SimualtedAnneal<param_type,6> BaseT;
// parameter
GyroSA(int t=7)
: m_distribution(param_type(-1.0/std::pow(10,t)),param_type(1.0/std::pow(10,t)))
{
// read in sample data
std::ifstream ifs("/home/georg/samples.txt");
std::string line;
if (ifs.is_open())
{
std::array<param_type,3> sample;
while (std::getline(ifs,line))
{
std::istringstream iss(line);
iss >> sample[0] >> sample[1] >> sample[2];
m_samples.push_back(sample);
//std::cout << "sample: " << line << std::endl;
}
}
}
virtual ~GyroSA()
{
}
void SaveTransformedSamples(const typename BaseT::ArrayT& params)
{
param_type x,y,z,px,py,pz,c;
param_type s1x = params[0];
param_type s2x = params[1];
param_type s2y = params[2];
param_type s3x = params[3];
param_type s3y = params[4];
param_type s3z = params[5];
std::ofstream ofs("/home/georg/transformed.txt");
for (size_t i = 0; i < m_samples.size(); ++i)
{
x = m_samples[i][0];
y = m_samples[i][1];
z = m_samples[i][2];
px = s3x * z + s2x * y + s1x * x;
py = s3y * z + s2y * y;
pz = s3z * z;
ofs << px << "\t"
<< py << "\t"
<< pz << std::endl;
}
}
protected:
// temperature
virtual param_type StartTemperature() override
{
return 1;
}
// iteration
virtual param_type MaxAllowedE() override
{
return 1e-6;
}
size_t GetMaxCycles() override
{
return 5000000;
}
// randomize data
virtual typename BaseT::ArrayT s0() override
{
typename BaseT::ArrayT params;
// we know the solution is somewhere here
// 0: s1x 1.0/1000
// 1: s2x 0
// 2: s2y 1.0/1000
// 3: s3x 0
// 4: s3y 0
// 5: s3z 1.0/1000
params[0] = 1.0/10000;
params[1] = 0;
params[2] = 1.0/10000;
params[3] = 0;
params[4] = 0;
params[5] = 1.0/10000;
return params;
}
virtual typename BaseT::ArrayT Neighbor(typename BaseT::ArrayT s) override
{
typename BaseT::ArrayT params = s;
for (size_t i = 0; i < params.size(); ++i)
{
params[i]+= m_distribution(m_rnd);
}
return params;
}
// get the current energy
virtual param_type E(const typename BaseT::ArrayT& params) override
{
// the sum of the square distance to g from all samples to this model
param_type ret = 0;
param_type x,y,z,px,py,pz,c;
param_type s1x = params[0];
param_type s2x = params[1];
param_type s2y = params[2];
param_type s3x = params[3];
param_type s3y = params[4];
param_type s3z = params[5];
for (size_t i = 0; i < m_samples.size(); ++i)
{
x = m_samples[i][0];
y = m_samples[i][1];
z = m_samples[i][2];
px = s3x * z + s2x * y + s1x * x;
py = s3y * z + s2y * y;
pz = s3z * z;
c = std::sqrt(px*px+py*py+pz*pz);
ret += std::pow(c-9.81,2);
}
return ret;
}
private:
// random generator
std::default_random_engine m_rnd;
std::uniform_real_distribution<param_type> m_distribution;
// sample data
std::vector< std::array<param_type,3> > m_samples;
};
// ------------------------------------------------
// Thread execute function
// ------------------------------------------------
template<class T>
void Execute(T& sa, typename T::ArrayT& r)
{
r = sa.Anneal();
}
template <class T>
struct ThreadData
{
thread_t th;
T sa;
typename T::ArrayT r;
};
// ------------------------------------------------
// main
// ------------------------------------------------
int main(int argc, const char** argv)
{
typedef long double calc_t;
typedef GyroSA<calc_t> sa_t;
using namespace std::placeholders;
std::cout << std::setprecision(12) << std::fixed ;
// create 8 thread
static const int nr_threads = 8;
std::array< ThreadData<sa_t>, nr_threads> td;
for (int i = 0; i < nr_threads; ++i)
{
td[i].th = thread_t(Execute<sa_t>, std::ref(td[i].sa), std::ref(td[i].r));
}
for (int i = 0; i < nr_threads; ++i)
{
td[i].th.join();
}
// get the lowest e
int res = -1;
calc_t lowest_e = std::numeric_limits<calc_t>::max();
for (int i = 0; i < nr_threads; ++i)
{
calc_t e = td[i].sa.Check(td[i].r);
if (e < lowest_e)
{
res = i;
lowest_e = e;
}
}
std::cout << "MinE: " << lowest_e << std::endl;
for (size_t i = 0; i < td[res].r.size(); ++i)
{
std::cout << "Param[" << i << "]="
<< td[res].r[i]
<< std::endl;
}
std::cout << std::endl;
// transform all samples to the results file
td[res].sa.SaveTransformedSamples(td[res].r);
}
Das ganze wurde mit C++11 umgesetzt. Kann aber auch mit MSVC10 und boost genutzt werden.
Je kleiner MinE (minimale Energie) ist, umso besser ist die Näherung.
3: Wie nutze ich das ganze dann auf dem µC?
Ganz einfach: Die 7 Parameter sind die oben genanten s1x etx.
x: x Wert aus dem Sensor
y: y Wert aus dem Sensor
z: z Wert aus dem Sensor
px: x Wert Kalibriert
py: y Wert Kalibriert
pz: z Wert Kalibriert
px = s3x * z + s2x * y + s1x * x;
py = s3y * z + s2y * y;
pz = s3z * z;
Die 7 Werte müssen also einmal für den Sensor ermittelt werden und können dann ins Programm auf den µC genutzt werden.
Bei Fragen .... fragen ;)
Gruß
Georg