Beschleunigung aus Winkel & Beschleunigung errechnen
Hi,
auch wenn der Titel im ersten Moment etwas komisch wirkt, hab ich eine plausible Frage dazu:
Ich messe mit einem stillstehendem Accelerometer die Beschleunigung in 3 Achsen (X, Y & Z). Daraus kann ich auch die Winkel in X & Y Richtung errechnen, bis dahin kein Problem.
Da ich allerdings mehrere Boards habe, muss / möchte ich den Ausgabewert bei einer Beschleunigung von 1g errechnen. Hintergrund ist, dass die Sensoren alle ein bisschen streuen und ich die MÖGLICHST GENAUEN Ausgabewerte brauche, da die Werte anschließend integriert werden sollen (Integral wird natürlich gestützt, aber um so genauer es ist, umso genauer ist im Endeffekt das Resultat).
Mein erster Gedanke war eine Art Dreisatz, allerdings komme ich nicht auf einen grünen Zweig...
Ich kenne: Beschleunigung (X, Y, Z) , Winkel (X , Y , (Z kann berechnet werden).
Ich will wissen: Beschleunigung bei 1g
Die Beschleunigung bei einem gegebenem Winkel kann ich über sin(Winkel) errechnen, den Winkel bei gegebener Beschleunigung über arcsin(Beschleunigung).
Ich hoffe, ihr könnt mir bei meinem kleinen Problem helfen.
Vielen Dank & Gruß
Chris
EDIT:
Achja, eigentlich dachte ich, es reicht, wenn ich Beschleunigung1g = BeschleunigungX + BeschleunigungY + BeschleunigungZ rechne, aber sowohl mit, als auch ohne den Beträgen funktionierts nicht ... Je weiter der Sensor geneigt ist, desto größer werden die Werte, obwohl doch die Beschleunigung immer 1g beträgt (bei Stillstand)?!
Liste der Anhänge anzeigen (Anzahl: 1)
Jetzt hab ich mal das Pferd von der anderen Seite aufgezäumt.
Ich habe 3 Sensoren Welche einen Bias haben, AD gewandelt werden und in meinem Beispiel 85° bzw. 95° zueinander versetzt sind.
Also ist die Welt zu Sensor Matrix
WCS := BIAS . AD . BASISVEKTOREN
Anhang 26455
Die untere Matrix stellt meine 5 "Messwerte" dar, welche je exakt 1g haben. 3 mal je parallel zu einer Achse und 2x exakt 45° geneigt. 3.132^2=9.81
Es zeigt exakt das Verhalten das der OP beschrieben hat.
Jetzt habe ich ein Model, das ich (hoffentlich) über die Samplewerte wiederherstellen kann. Ich benötige dann "nur" die 9 Parameter der S2W Matrix.
Gruß
Georg
Liste der Anhänge anzeigen (Anzahl: 2)
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:
Anhang 26500
Standardabweichung und Mittelwert der Länge der "g" Vektoren:
Anhang 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
Code:
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:
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
