ATNEL tech-forum 2015-07-21T11:55:05+01:00 https://forum.atnel.pl/feed.php?f=23&t=12022&mode 2015-07-21T11:55:05+01:00 2015-07-21T11:55:05+01:00 https://forum.atnel.pl/viewtopic.php?t=12022&p=135095#p135095 <![CDATA[Re: Filtr Kalmana Fuzja Akcelerometru i Żyroskopu]]> Statystyki: Napisane przez Daniel880 — 21 lip 2015, o 11:55

]]> 2015-06-15T17:25:26+01:00 2015-06-15T17:25:26+01:00 https://forum.atnel.pl/viewtopic.php?t=12022&p=131830#p131830 <![CDATA[Re: Filtr Kalmana Fuzja Akcelerometru i Żyroskopu]]> krzysssztof napisał(a):

Taki to obserwator stanu sobie kolega implementuje :)

Do tego tematu masz jakieś zapytania, czy tylko wstawiasz dobre rozwiązanie dla potomności ? (z chęcią kiedyś skorzystam).


To rozwiązanie działało z modułem GY-80 dobrze stabilizowało kąt. Problemem w implementacji na AVR są liczby float, dlatego w przyszłości spróbuje przekształcić kod aby liczył na liczbach całkowitych. Niestety filtr trzeba dostroić do indywidualnego przypadku złe ustawienie może generować opóźnienia które uniemożliwią wykorzystanie go w aplikacjach o szybkiej zmianie kąta.

Statystyki: Napisane przez krzyhu0905gmail — 15 cze 2015, o 17:25

]]> 2015-06-15T16:08:11+01:00 2015-06-15T16:08:11+01:00 https://forum.atnel.pl/viewtopic.php?t=12022&p=131828#p131828 <![CDATA[Re: Filtr Kalmana Fuzja Akcelerometru i Żyroskopu]]>

Do tego tematu masz jakieś zapytania, czy tylko wstawiasz dobre rozwiązanie dla potomności ? (z chęcią kiedyś skorzystam).

Statystyki: Napisane przez krzysssztof — 15 cze 2015, o 16:08

]]> 2015-06-15T15:42:17+01:00 2015-06-15T15:42:17+01:00 https://forum.atnel.pl/viewtopic.php?t=12022&p=131824#p131824 <![CDATA[Filtr Kalmana Fuzja Akcelerometru i Żyroskopu]]> * calman.h
*
* Created: 2014-02-10 20:06:30
* Author: KRZYCHUX
*/


#ifndef CALMAN_H_
#define CALMAN_H_

//#define dt //przeniesione do confing

/*struktura stanu*/
struct typ_stan
{
float theta;//odchylenie//
float omega;//predkosc odchylania//
float pomiar_theta;//odchylenie//
float pomiar_omega;//predkosc katowa//
};
struct typ_stan stan;
struct typ_stan stan1;

struct typ_kalman
{
float Q;//wartosc przekatnej macierzy
float R;//wariancja pomiaru
float theta;//odchylenie
float omega_previous;
float omega;//predkosc katowa
float g_bias;//dryft zyroskopu
float P11;//wartosc macierzy kowariancji P
float P13;
float P21;
float P31;
float P33;
float K1;//wartosc macierzy K
float K2;
float K3;
};

struct typ_kalman kalm;
struct typ_kalman kalm1;




void calman_init_roll(void);
void calman_init_pitch(void);

float calman_filtr_roll(float akcel,float gyro);
float calman_filtr_pitch(float akcel,float gyro);


#endif /* CALMAN_H_ */[/syntax]
[syntax=c]/*
* calman.c
*
* Created: 2014-02-10 20:06:14
* Author: KRZYCHUX
*/
#include <avr/io.h>
#include "calman.h"
#include "confing.h"

void calman_init_roll(void)
{
//stan
stan.theta=0.0;
stan.omega=0.0;
stan.pomiar_theta=0.0;
stan.pomiar_omega=0.0;
//kalman
kalm.Q=0.00000005;//0.00000009; //mniejsza liczba zmniejsza wrazliwosc na przyspieszenia
kalm.R=1.0;//
kalm.theta=0.0;
kalm.omega_previous=0.0;
kalm.omega=0.0;
kalm.g_bias=0.0;
kalm.P11=kalm.R;
kalm.P13=0.0;
kalm.P21=0.0;
kalm.P31=0.0;
kalm.P33=0.0;
kalm.K1=0.0;
kalm.K2=0.0;
kalm.K3=0.0;
}
void calman_init_pitch(void)
{
//stan
stan1.theta=0.0;
stan1.omega=0.0;
stan1.pomiar_theta=0.0;
stan1.pomiar_omega=0.0;
//kalman
kalm1.Q=0.00000005;//0.00000009; //mniejsza liczba zmniejsza wrazliwosc na przyspieszenia
kalm1.R=1.0;//
kalm1.theta=0.0;
kalm1.omega_previous=0.0;
kalm1.omega=0.0;
kalm1.g_bias=0.0;
kalm1.P11=kalm.R;
kalm1.P13=0.0;
kalm1.P21=0.0;
kalm1.P31=0.0;
kalm1.P33=0.0;
kalm1.K1=0.0;
kalm1.K2=0.0;
kalm1.K3=0.0;
}
/*
ustawienia dla Q
0.0001 szybka reakcja na plyniecie zyroskopu duza wrazliwosc na przyspieszenie układu
0.00001 -podobnie
0.000001-w miare szybka kompensacja dryftu +-15st wrazliowsc na przyspieszenia
0.0000001-wolna kompencajca dryftu +- 3 st wrazliwosci na przyspieszenia
*/
float calman_filtr_roll(float akcel,float gyro)
{
stan.pomiar_theta=akcel;//(float)deg_roll_pitch[ROLL];//odczyt kata z akcel do filtra
stan.pomiar_omega=(0.0-gyro)*0.061052;//(0.0-value_gyro_axis[ROLL])*0.061052; // przy rozdzielczosci 2000st/sec





kalm.theta=((stan.pomiar_omega-kalm.g_bias)*dt)+kalm.theta;


kalm.omega=stan.pomiar_omega-kalm.g_bias;

//kalm.omega_previous=kalm.omega;



//Pk=Ap_{k-1}A^T+Q
kalm.P11 = kalm.P11-kalm.P31*dt+kalm.P33*dt*dt-kalm.P13*dt+kalm.Q;
kalm.P13=kalm.P13-kalm.P33*dt;
kalm.P21=kalm.P33*dt-kalm.P31;
kalm.P31=kalm.P31-kalm.P33*dt;
kalm.P33=kalm.P33+kalm.Q;


//KOREKCJA

//Kk=PkH^T(HPk^T+R)^-1

kalm.K1=kalm.P11*(1.0/(kalm.P11+kalm.R));
kalm.K2=kalm.P21*(1.0/(kalm.P11+kalm.R));
kalm.K3=kalm.P31*(1.0/(kalm.P11+kalm.R));

//xk=xk-K(zk-Hxk)

kalm.theta=kalm.theta+kalm.K1*(stan.pomiar_theta-kalm.theta);
kalm.omega=kalm.omega+kalm.K2*(stan.pomiar_theta-kalm.theta);
kalm.g_bias=kalm.g_bias+kalm.K3*(stan.pomiar_theta-kalm.theta);

//Pk=(1-Kkh)Pk

kalm.P11=(1.0-kalm.K1)*kalm.P11;
kalm.P13=(1.0-kalm.K1)*kalm.P13;
kalm.P21=kalm.P21-kalm.P11*kalm.K2;
kalm.P31=kalm.P31-kalm.P11*kalm.K3;
kalm.P33=kalm.P33-kalm.P13*kalm.K3;

//aktualizacja wektora stanu

stan.theta=kalm.theta;
stan.omega=kalm.omega;

return stan.theta;
}
float calman_filtr_pitch(float akcel,float gyro)
{
stan1.pomiar_theta=akcel;//(float)deg_roll_pitch[ROLL];//odczyt kata z akcel do filtra
stan1.pomiar_omega=(0.0-gyro)*0.061052;//(0.0-value_gyro_axis[ROLL])*0.061052; // przy rozdzielczosci 2000st/sec





kalm1.theta=((stan1.pomiar_omega-kalm1.g_bias)*dt)+kalm1.theta;


kalm1.omega=stan1.pomiar_omega-kalm1.g_bias;

//kalm.omega_previous=kalm.omega;



//Pk=Ap_{k-1}A^T+Q
kalm1.P11 = kalm1.P11-kalm1.P31*dt+kalm1.P33*dt*dt-kalm1.P13*dt+kalm1.Q; i Q
kalm1.P13=kalm1.P13-kalm1.P33*dt;
kalm1.P21=kalm1.P33*dt-kalm1.P31;
kalm1.P31=kalm1.P31-kalm1.P33*dt;
kalm1.P33=kalm1.P33+kalm1.Q;


//KOREKCJA

//Kk=PkH^T(HPk^T+R)^-1

kalm1.K1=kalm1.P11*(1.0/(kalm1.P11+kalm1.R));
kalm1.K2=kalm1.P21*(1.0/(kalm1.P11+kalm1.R));
kalm1.K3=kalm1.P31*(1.0/(kalm1.P11+kalm1.R));

//xk=xk-K(zk-Hxk)

kalm1.theta=kalm1.theta+kalm1.K1*(stan1.pomiar_theta-kalm1.theta);
kalm1.omega=kalm1.omega+kalm1.K2*(stan1.pomiar_theta-kalm1.theta);
kalm1.g_bias=kalm1.g_bias+kalm1.K3*(stan1.pomiar_theta-kalm1.theta);

//Pk=(1-Kkh)Pk

kalm1.P11=(1.0-kalm1.K1)*kalm1.P11;
kalm1.P13=(1.0-kalm1.K1)*kalm1.P13;
kalm1.P21=kalm1.P21-kalm1.P11*kalm1.K2; //?
kalm1.P31=kalm1.P31-kalm1.P11*kalm1.K3; //?
kalm1.P33=kalm1.P33-kalm1.P13*kalm1.K3;//?

//aktualizacja wektora stanu

stan1.theta=kalm1.theta;
stan1.omega=kalm1.omega;

return stan1.theta;
}[/syntax]

Statystyki: Napisane przez krzyhu0905gmail — 15 cze 2015, o 15:42

]]>