Mpu6050 kalman filtresi algoritması

[remzi]

Şunu kesinlikle belirteyim Kalmam filtresi acılarla ilgili bir şey değildir. Sadece yapılan ölçümleri düzenler. Önce bu ayırımı yapmak lazım.  Ölçümleri ani sıçrama ve parazitleri filtreler. Bunu lineer bir şekilde önceki ölçülere göre yapar.

Peki filtreleme bir girişime göre yapılıyor yoksa daha fazla girişe göremi?
Ben tek kanaldan ADC okuyorum. Parazitli bir giriş. Bunu nasıl kalman ile filtrelerim?

[görkem]

 2 veriyi tek denkleme nasıl çevirebiliriz gyro ve ivme sensöründen gelen verileri tek açı bilgimizi elde etmek için

[muhittin_kaplan]

http://www.muhittinkaplan.com/?p=29

buradaki K ve R sabitleriyle oynarak filtreyi yapabilirsiniz,

[boreas]

ivme dediğin vektörel bir bilgi. A noktasından B noktasına ne hızda hareket ettiğini verir. Eğer sen bu hızın açısını merak ediyorsan. A noktasındaki x,y,z bilgileri ile B noktasındaki xyz bilgileri arasında ivmene göre bir doğru olduğunu varsayıp 3 boyutlu analitik düzlemde doğrunun açısını bulabilirsin. Ya da ben böyle yapardım

[robomaster]

Peki filtreleme bir girişime göre yapılıyor yoksa daha fazla girişe göremi?
Ben tek kanaldan ADC okuyorum. Parazitli bir giriş. Bunu nasıl kalman ile filtrelerim?

Her bir eksen için x , y , z ( yada hangilerini kullanacaksan) ayrı ayrı kalman filtren çalışacak.
Açıları bulmak filtre çıkışından gelen okumalar ile ilgili,  kalman ile ilgili değil. Gyro ise ap ayrı bir mevzu. Şimdilik sadece
ivmeölçer ile statik durumda açıları bulmak ile başla bence.
Muhittin hocanın sitesindeki uygulamaları bir incele.

[feridund]

Arduino için örnek kod var mı acaba ...

[zengdai]

özür dilerim cevap vermekte biraz geç kaldım. Forumda cevaplar geç gelince çekilen sıkıntıyı bilirim
kullanılan sensörler itg3200 ve adxl340 sen bunu kendi sensürüne uyarlarsın.tüm arduino kodu altta;
kodlarda Complimentary ve kalman filtresini karşılaştırmış, yani her iki filtre tipi var.Biraz uğraştıktan sonra meseleyi çözersin.

Bu kısımda başta sensörlerin kalibrasyonu yapılıyor;

for (i = 0; i < 100; i += 1) {
getGyroscopeReadings(Gyro_output);
getAccelerometerReadings(Accel_output);
Gyro_cal_x_sample += Gyro_output[0];
Gyro_cal_y_sample += Gyro_output[1];
Gyro_cal_z_sample += Gyro_output[2];
Accel_cal_x_sample += Accel_output[0];
Accel_cal_y_sample += Accel_output[1];
Accel_cal_z_sample += Accel_output[2];
delay(50);
}
Gyro_cal_x = Gyro_cal_x_sample / 100;
Gyro_cal_y = Gyro_cal_y_sample / 100;
Gyro_cal_z = Gyro_cal_z_sample / 100;
Accel_cal_x = Accel_cal_x_sample / 100;
Accel_cal_y = Accel_cal_y_sample / 100;
Accel_cal_z = (Accel_cal_z_sample / 100) - 256; // Raw Accel output in z-direction is in 256 LSB/g due to gravity
and must be off-set by approximately 256}

Yani mesela Gyro_cal_x değeri kalibrayon sonucunda bulunan ofsett miktarı.

#include <Wire.h>
int Gyro_output[3], Accel_output[3];
float dt = 0.02;
float Gyro_cal_x, Gyro_cal_y, Gyro_cal_z, Accel_cal_x, Accel_cal_y, Accel_cal_z;
float Gyro_pitch = 0; //Initialize value
float Accel_pitch = 0; //Initial Estimate
float Predicted_pitch = 0; //Initial Estimate
float Gyro_roll = 0; //Initialize value
float Accel_roll = 0; //Initialize value
float Predicted_roll = 0; //Output of Kalman filter
float Q = 0.1; // Prediction Estimate Initial Guess
float R = 5; // Prediction Estimate Initial Guess
float P00 = 0.1; // Prediction Estimate Initial Guess
float P11 = 0.1; // Prediction Estimate Initial Guess
float P01 = 0.1; // Prediction Estimate Initial Guess
float Kk0, Kk1;
float Comp_pitch;
float Comp_roll;
unsigned long timer;
unsigned long time;
void writeTo(byte device, byte toAddress, byte val) {
Wire.beginTransmission(device);
Wire.write(toAddress);
Wire.write(val);
Wire.endTransmission();
}
// Adapted from Arduino.cc/forums for reading in sensor data
void readFrom(byte device, byte fromAddress, int num, byte result[]) {
Wire.beginTransmission(device);
Wire.write(fromAddress);
Wire.endTransmission();
Wire.requestFrom((int)device, num);
int i = 0;
while(Wire.available()) {
result[i] = Wire.read();
i++;
}
}
void getGyroscopeReadings(int Gyro_output[]) {
byte buffer[6];
readFrom(0x68,0x1D,6,buffer);
Gyro_output[0] = (((int)buffer[0]) << 8 ) | buffer[1];
Gyro_output[1] = (((int)buffer[2]) << 8 ) | buffer[3];
Gyro_output[2] = (((int)buffer[4]) << 8 ) | buffer[5];
}
void getAccelerometerReadings(int Accel_output[]) {
byte buffer[6];
readFrom(0x53,0x32,6,buffer);
Accel_output[0] = (((int)buffer[1]) << 8 ) | buffer[0];
Accel_output[1] = (((int)buffer[3]) << 8 ) | buffer[2];
Accel_output[2] = (((int)buffer[5]) << 8 ) | buffer[4];
}
void setup() {
int Gyro_cal_x_sample = 0;
int Gyro_cal_y_sample = 0;
int Gyro_cal_z_sample = 0;
int Accel_cal_x_sample = 0;
int Accel_cal_y_sample = 0;
int Accel_cal_z_sample = 0;
int i;
delay(5);
Wire.begin();
Serial.begin(115200); // Registers set
writeTo(0x53,0x31,0x09); //Set accelerometer to 11bit, +/-4g
writeTo(0x53,0x2D,0x08); //Set accelerometer to measure mode
writeTo(0x68,0x16,0x1A); //Set gyro +/-2000deg/sec and 100Hz Low Pass Filter
writeTo(0x68,0x15,0x09); //Set gyro 100Hz sample rate
Serial.print("time\tGyro_pitch\tAccel_pitch\tComp_pitch\tPredicted_pitch\tGyro_roll\tAccel_roll\tComp_roll\tPre
dicted_roll\ty\tp\tr\n");
delay(100);
for (i = 0; i < 100; i += 1) {
getGyroscopeReadings(Gyro_output);
getAccelerometerReadings(Accel_output);
Gyro_cal_x_sample += Gyro_output[0];
Gyro_cal_y_sample += Gyro_output[1];
Gyro_cal_z_sample += Gyro_output[2];
Accel_cal_x_sample += Accel_output[0];
Accel_cal_y_sample += Accel_output[1];
Accel_cal_z_sample += Accel_output[2];
delay(50);
}
Gyro_cal_x = Gyro_cal_x_sample / 100;
Gyro_cal_y = Gyro_cal_y_sample / 100;
Gyro_cal_z = Gyro_cal_z_sample / 100;
Accel_cal_x = Accel_cal_x_sample / 100;
Accel_cal_y = Accel_cal_y_sample / 100;
Accel_cal_z = (Accel_cal_z_sample / 100) - 256; // Raw Accel output in z-direction is in 256 LSB/g due to gravity
and must be off-set by approximately 256
}
void loop() {
timer = millis();
getGyroscopeReadings(Gyro_output);
getAccelerometerReadings(Accel_output);
Accel_pitch = atan2((Accel_output[1] - Accel_cal_y) / 256, (Accel_output[2] - Accel_cal_z) / 256) * 180 / PI;
Gyro_pitch = Gyro_pitch + ((Gyro_output[0] - Gyro_cal_x) / 14.375) * dt;
//if(Gyro_pitch<180) Gyro_pitch+=360; // Keep within range of 0-180 deg to match Accelerometer output
//if(Gyro_pitch>=180) Gyro_pitch-=360;
Comp_pitch = Gyro_pitch;
Predicted_pitch = Predicted_pitch + ((Gyro_output[0] - Gyro_cal_x) / 14.375) * dt; // Time Update step 1
Accel_roll = atan2((Accel_output[0] - Accel_cal_x) / 256, (Accel_output[2] - Accel_cal_z) / 256) * 180 / PI;
Gyro_roll = Gyro_roll - ((Gyro_output[1] - Gyro_cal_y) / 14.375) * dt;
//if(Gyro_roll<180) Gyro_roll+=360; // Keep within range of 0-180 deg
//if(Gyro_roll>=180) Gyro_roll-=360;
Comp_roll = Gyro_roll;
Predicted_roll = Predicted_roll - ((Gyro_output[1] - Gyro_cal_y) / 14.375) * dt; // Time Update step 1
P00 += dt * (2 * P01 + dt * P11); // Projected error covariance terms from derivation result: Time Update step 2
P01 += dt * P11; // Projected error covariance terms from derivation result: Time Update step 2
P00 += dt * Q; // Projected error covariance terms from derivation result: Time Update step 2
P11 += dt * Q; // Projected error covariance terms from derivation result: Time Update step 2
Kk0 = P00 / (P00 + R); // Measurement Update step 1
Kk1 = P01 / (P01 + R); // Measurement Update step 1
Predicted_pitch += (Accel_pitch - Predicted_pitch) * Kk0; // Measurement Update step 2
Predicted_roll += (Accel_roll - Predicted_roll) * Kk0; // Measurement Update step 2
P00 *= (1 - Kk0); // Measurement Update step 3
P01 *= (1 - Kk1); // Measurement Update step 3
P11 -= Kk1 * P01; // Measurement Update step 3
float alpha = 0.98;
Comp_pitch = alpha*(Comp_pitch+Comp_pitch*dt) + (1.0 - alpha)*Accel_pitch; // Complimentary filter
Comp_roll = alpha*(Comp_roll+Comp_roll*dt) + (1.0 - alpha)*Accel_roll; // Complimentary filter
float angle_z = Gyro_output[2];
time=millis();
Serial.print(time);
Serial.print("\t");
Serial.print(Gyro_pitch);
Serial.print("\t");
Serial.print(Accel_pitch);
Serial.print("\t");
Serial.print(Comp_pitch);
Serial.print("\t");
Serial.print(Predicted_pitch);
Serial.print("\t");
Serial.print(Gyro_roll);
Serial.print("\t");
Serial.print(Accel_roll);
Serial.print("\t");
Serial.print(Comp_roll);
Serial.print("\t");
Serial.print(Predicted_roll);
Serial.print("\n");
timer = millis() - timer;
timer = (dt * 1000) - timer;
delay(timer);
}

Dualarını eksik etme

[görkem]

@zengdai hiç önemli değil ilginiz ve alakanız için teşekkür ederim sağolun eksik etmicem

[fatal16]

Arduino'da yapılmış çalışma sizin için örnek olabilir.

Buradaki kütüphaneleri ve kodları kullanacağınız derleyiciye göre uyarlarsanız çalışır.

https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter/tree/master/IMU/MPU6050

[görkem]

Teşekkür ederim ilginiz için