private void button41_Click(object sender, EventArgs e)
{
xIMU_1.Reset_Pos();
ThreeDPoseChart.Clear();
}
private void UpdateWPFChart()
{
zaehler++;
ThreeDPoseChart.plot_next(xIMU_1.DataGatherer.KalEstLinPos.CurrentMeasurement, xIMU_2.DataGatherer.KalEstLinPos.CurrentMeasurement,
zaehler, xIMU_1.DataGatherer.RotMatrix(), xIMU_1.DataGatherer.RotMatrix());
}