ادغام IMU و GPS با فیلتر کالمن برای UGV پایدار (فارسی)

فیلتر کالمن ساده — Complementary به Kalman

برای مبتدی‌ها ابتدا یک فیلتر کالمن ۱D برای ترکیب GPS Lat با Dead Reckoning پیاده می‌کنیم:

// فیلتر کالمن ۱ بعدی برای موقعیت X
struct Kalman1D {
  float x;       // تخمین موقعیت
  float P;       // واریانس خطا
  float Q=0.01;  // نویز پروسس (IMU drift)
  float R=2.5;   // نویز اندازه‌گیری (GPS noise متر)

  float update(float measurement, float prediction){
    // Predict
    P += Q;
    // Update
    float K = P / (P + R);          // Kalman Gain
    x = prediction + K*(measurement - prediction);
    P = (1 - K) * P;
    return x;
  }
};

Kalman1D kx, ky;
float estX=0, estY=0;

void loop(){
  // از Odometry/IMU تخمین حرکت
  float predX = estX + vx * dt;
  float predY = estY + vy * dt;

  if(gps.location.isUpdated()){
    // GPS موقعیت جدید (متر از مبدأ)
    float gpsX = (gps.location.lat() - homeLat) * 111320.0;
    float gpsY = (gps.location.lng() - homeLng) * 111320.0 * cos(radians(homeLat));
    estX = kx.update(gpsX, predX);
    estY = ky.update(gpsY, predY);
  } else {
    estX = predX; estY = predY;  // فقط IMU
  }
}

جدول مقایسه روش‌های Fusion

• کتابخانه Kalman Filter Arduino — GitHub
• آپارات: فیلتر کالمن فارسی