Activity - SVentas

Андрей_Гапонов:

Quaternion based attitude estimation loop runs at 500 Hz Это очень медленно

На днях будет 667 Hz как у STorM32 😉.

sanik:

А осторожность в чем заключается?

Я каждый раз всего не тестирую потому могут быть косяки.
А так все стандартно - не подключать USART1 и батарейку одновременно, нерекомендую питать контролер от напряжения больше 9В, выставить Dead-Time по меньше но смотреть чтобы FETы не перегрелись. Ну и наверно все…
Follow me работает. PID настраивать как в STorM32. Настройки в районе 30…120. Если все пойдет по плану, скоро прикручю второй IMU.
Чуть больше информации здесь а прошивка и код тут.

Я тоже написал свою прошивку для EvvGC. У меня PID контроллер очень похож на тот который используеться в STorM32.
Прошивку можно скачать тут.
Это эксперементальная прошивка потому надо быть осторожним.

SergDoc:

далее да согласен, и момент, угол с акселя также можно назвать константой, он ничем не корректируется и не проверяется…

У меня впечатление что Вы меня не понимаете. Угол с акселя x_angle и x_bias это не константы. KX_0 и KX_1- константы. Вот их не надо все время вычеслять. Ладно, завтра или после завтра я вам напишу простой пример. Тогда надеюсь что будет ясно 😃.

SergDoc:

это погрешности датчиков… они и есть константы и будут зависеть только от датчиков
далее да согласен, и момент, угол с акселя также можно назвать константой, он ничем не корректируется и не проверяется…

Тогда зачем надо делать такую кучю вычислений, когда можно одич раз вычислить КХ и применить простую формулу low-pass фильтра? 😃
Ответ я знаю 😉 - чтобы написать что это “фильтр Калмана”. Но это смешно 😆.
P. S.
Это не Вы смешной, это автор кода 😉.

rual:

Прошу прощения за невежество, но вроде как в коде нет оценки правильности подбора коэффициента и его корректировки.

Совершенно верно.
Далее о другом примере:

  float Q_angleX  =  0.001;
  float Q_gyroX   =  0.003;
  float R_angleX  =  0.03;

  float x_angle = 0;
  float x_bias = 0;
  float PX_00 = 0, PX_01 = 0, PX_10 = 0, PX_11 = 0;
  float dtX, yX, SX;
  float KX_0, KX_1;

  float kalmanCalculateX(float newAngle, float newRate,int looptime)
  {
      dtX = float(looptime)/1000;
      x_angle += dtX * (newRate - x_bias);
      PX_00 +=  - dtX * (PX_10 + PX_01) + Q_angleX * dtX;
      PX_01 +=  - dtX * PX_11;
      PX_10 +=  - dtX * PX_11;
      PX_11 +=  + Q_gyroX * dtX;

      yX = newAngle - x_angle;
      SX = PX_00 + R_angleX;
      KX_0 = PX_00 / SX;
      KX_1 = PX_10 / SX;

      x_angle +=  KX_0 * yX;
      x_bias  +=  KX_1 * yX;
      PX_00 -= KX_0 * PX_00;
      PX_01 -= KX_0 * PX_01;
      PX_10 -= KX_1 * PX_00;
      PX_11 -= KX_1 * PX_01;

      return x_angle;
  }

Это почти тоже самое что было ранее (наверное еще один „гениальный Калмановский“ код brm?). Ладно 😃. Посмотрите где в коде меняются Q_angleX, Q_gyroX и R_angleX. Нигде! Значит это константы.
А теперь посмотрите где в коде PX_00, PX_01, PX_10, PX_11, SX, KX_0 и KX_1 зависит от x_angle или newRate или newAngle или x_bias т.е. от данных. Опять нигде! Значит и это будут константы (не совсем, так как они зависят от времени, см. дальше).
В этом коде добавлена зависимость ФВЧ от времени (если sampling rate непостоянный). Вот и все.
Я хочу сказать что матричные элементы PX, SX и KX зависит только от констант Q_angleX, Q_gyroX, R_angleX ну и от dtX. От данных они НЕ ЗАВИСЯТ! Здесь нет прогноза данных и нет ни какой физической или мат. модели процесса. И быт ни может, потому что нельзя предсказать ни гиро ни акселя из их же самих предыдущих данных.
SegDoc, eсли что не ясно, то посмотрите тот код, что был ранее. Он проще (только 5 строчек 😃 ).

Здравствуйте SergDoc. Извиняюсь за свой русский 😃.
Я долго читаю эту ветку и вижу, что Вы пишите про фильтр Калмана на одну ось у Naze32. Тогда мне становиться очень грустно, потому что такой “фильтр Калмана” есть у MultiWii (не Naze32, а MultiWii) уже давно, но там он называется low-pass filter (ФВЧ). Я надеюсь что Вы поймете почему так.
Взгляните на код, которий Вы сами выложили:


/*
 * kalman.c
 *
 *  Created on: 25.11.2012
 *      Author: rob
 */

#include "stdint.h"
#include "kalman_simple.h"

typedef struct
{
	float q;	// process noise covariance
	float r;	// measurement noise covariance
	float x;	// value
	float p;	// estimation error covariance
} kalman_state;

// accelerometer
kalman_state kax;
kalman_state kay;
kalman_state kaz;

// gyro
kalman_state kgx;
kalman_state kgy;
kalman_state kgz;

void kalman_init(kalman_state* state, float q, float r, float p, float intial_value)
{
	state->q = q;
	state->r = r;
	state->p = p;
	state->x = intial_value;
}

void kalman_update(kalman_state* state, float measurement)
{
	// prediction update
	// omit x = x
	float k; // kalman gain
	state->p = state->p + state->q;

	// measurement update
	k = state->p / (state->p + state->r);
	state->x = state->x + k * (measurement - state->x);
	state->p = (1 - k) * state->p;
}

void initKalmanAccel(float ax, float ay, float az)
{
// small jakub frame
#define Q 0.0625		// process noise covariance
#define	R 1.0			// measurement noise covariance
#define P 0.22			// estimation error covariance
	kalman_init(&kax, Q, R, P, ax);
	kalman_init(&kay, Q, R, P, ay);
	kalman_init(&kaz, Q, R, P, az);
#undef Q
#undef R
#undef P
}

void initKalmanGyro(float gx, float gy, float gz)
{

// small jakub frame
#define Q 1.0 		// process noise covariance
#define	R 0.0625	// measurement noise covariance
#define P 0.22		// estimation error covariance
	kalman_init(&kgx, Q, R, P, gx);
	kalman_init(&kgy, Q, R, P, gy);
	kalman_init(&kgz, Q, R, P, gz);
#undef Q
#undef R
#undef P
}

inline int kalman_filter_step(kalman_state* state, int16_t val)
{
	float measurement = (float) val;
	kalman_update(state, measurement);
	return (state->x + 0.5f);
}

void accKalmanFilterStep(int16_t accels[3])
{
	static int _init = 0;
	if (!_init)
	{
		_init = 1;
		initKalmanAccel(accels[0], accels[1], accels[2]);
	}
	else
	{
		accels[0] = kalman_filter_step(&kax, accels[0]);
		accels[1] = kalman_filter_step(&kay, accels[1]);
		accels[2] = kalman_filter_step(&kaz, accels[2]);
	}
}

void gyroKalmanFilterStep(int16_t gyros[3])
{
	static int _init = 0;
	if (!_init)
	{
		_init = 1;
		initKalmanGyro(gyros[0], gyros[1], gyros[2]);
	}
	else
	{
		gyros[0] = kalman_filter_step(&kgx, gyros[0]);
		gyros[1] = kalman_filter_step(&kgy, gyros[1]);
		gyros[2] = kalman_filter_step(&kgz, gyros[2]);
	}
}

Там нет ничего интересного кроме этой ф-ции (всего 5 строк):

void kalman_update(kalman_state* state, float measurement)
{
	// prediction update
	// omit x = x
	float k; // kalman gain
	state->p = state->p + state->q;

	// measurement update
	k = state->p / (state->p + state->r);
	state->x = state->x + k * (measurement - state->x);
	state->p = (1 - k) * state->p;
}

В этом коде state->p, state->q и state->r есть КОНСТАНТЫ, которые не зависят ни от времени ни от данных. Они зависят только одна от другой. Потому k (или kalman gain), тоже КОНСТАНТА. Она обретает свое стабильное значение (как и state->p) спустя пару десятков итераций и больше не меняется. Если не верите и не хотите считать сами, то выведите значение k и/или state->p на debug MultiWiiConf и увидите.
Потому что k есть КОНСТАНТА

state->x = state->x + k * (measurement - state->x);

есть обыкновенный low-pass filter как и в MultiWii. Вот Вам и “фильтр Калмана” на одну ось…
Я когда то решил поспорить с brm на эту тему, но увидил что с ним спорить бесполезно.