Algoritma Sensor Fusion dan Implementasi dalam Python

 Algoritma Sensor Fusion dan Implementasi dalam Python

Abstrak Sensor Fusion adalah teknik menggabungkan data dari berbagai sensor untuk meningkatkan akurasi dan keandalan sistem. Dalam artikel ini, kita akan membahas konsep dasar Sensor Fusion, algoritma yang digunakan seperti Kalman Filter, Complementary Filter, dan Particle Filter, serta implementasinya menggunakan bahasa pemrograman Python.

Kata Kunci: Sensor Fusion, Kalman Filter, Complementary Filter, Particle Filter, Python, Estimasi Data

---

1. Pendahuluan

Sensor Fusion digunakan dalam berbagai aplikasi seperti kendaraan otonom, robotika, dan sistem navigasi. Dengan menggabungkan data dari beberapa sensor seperti IMU (Inertial Measurement Unit), GPS, dan LiDAR, sistem dapat memperoleh estimasi keadaan yang lebih akurat.

2. Algoritma Sensor Fusion

2.1 Kalman Filter

Kalman Filter adalah algoritma berbasis probabilistik yang digunakan untuk mengestimasi keadaan sistem dinamis dengan mengurangi ketidakpastian pengukuran sensor. Algoritma ini bekerja dengan pendekatan dua tahap:

1. Prediksi (Prediction Step):
   • Menggunakan model sistem untuk memperkirakan keadaan berikutnya.
   • Memperhitungkan ketidakpastian berdasarkan noise sistem.
2. Koreksi (Update Step):
   • Memperbaiki estimasi berdasarkan pengukuran sensor.
   • Menggunakan perhitungan bobot optimal melalui matriks kovariansi untuk meminimalkan kesalahan estimasi.

Persamaan Dasar Kalman Filter

1. Prediksi keadaan:

$${\widehat{x}}_{k\mathrm{\mid }k-1}=A{\widehat{x}}_{k-1\mathrm{\mid }k-1}+B{u}_k$$

2. Prediksi kovariansi kesalahan:

$$P_{k\mathrm{\mid }k-1}=A{P}_{k-1\mathrm{\mid }k-1}{A}^T+Q$$

3. Perhitungan gain Kalman:

$$K_k=P_{k\mathrm{\mid }k-1}{H}^T(H{P}_{k\mathrm{\mid }k-1}{H}^T+R{)}^{-1}$$

4. Koreksi estimasi berdasarkan pengukuran:

$${\widehat{x}}_{k\mathrm{\mid }k}={\widehat{x}}_{k\mathrm{\mid }k-1}+K_k(z_k-H{\widehat{x}}_{k\mathrm{\mid }k-1})$$

5. Koreksi kovariansi kesalahan:

$$P_{k\mathrm{\mid }k}=(I-K_{k}H)P_{k\mathrm{\mid }k-1}$$

Kalman Filter banyak digunakan dalam navigasi GPS, pemrosesan sinyal, dan sistem kendali otomatis untuk estimasi yang lebih akurat.

2.2 Complementary Filter

Complementary Filter adalah teknik sederhana yang menggabungkan data dari sensor dengan frekuensi tinggi dan rendah untuk mengurangi noise.

Teknik ini sering digunakan dalam aplikasi pengolahan sinyal, seperti estimasi sudut dalam sistem navigasi berbasis IMU (Inertial Measurement Unit). Dalam IMU, akselerometer dapat digunakan untuk mengukur orientasi berdasarkan gravitasi, tetapi sangat rentan terhadap noise jangka pendek. Sebaliknya, giroskop memberikan data kecepatan sudut dengan akurasi tinggi dalam jangka pendek tetapi mengalami drift dalam jangka panjang.

Complementary Filter bekerja dengan memanfaatkan kedua sensor ini:

• Menggunakan akselerometer untuk koreksi drift jangka panjang.
• Menggunakan giroskop untuk perhitungan orientasi jangka pendek.

Persamaan Complementary Filter

data estimasi sudut $\theta$ dapat dihitung menggunakan rumus berikut:

$$\theta =\alpha ({\theta }_{sebelumnya}+\omega \mathrm{\Delta }t)+(1-\alpha )({\theta }_{akselerometer})$$

Di mana:

• $\alpha$ adalah parameter filter (biasanya antara 0.90 - 0.98),
• $\omega$ adalah kecepatan sudut dari giroskop,
• ${\theta }_{akselerometer}$ adalah sudut yang dihitung dari akselerometer.

Complementary Filter sering digunakan dalam sistem navigasi drone, robot, dan kendaraan untuk estimasi sudut yang stabil dan akurat.

2.3 Particle Filter

Particle Filter digunakan untuk estimasi keadaan sistem non-linear dan non-Gaussian dengan pendekatan berbasis partikel probabilistik. Particle Filter bekerja dengan menggunakan sejumlah besar partikel untuk merepresentasikan distribusi probabilitas keadaan sistem. Langkah-langkah utama dalam Particle Filter adalah:

1. Inisialisasi: Menentukan sejumlah partikel dengan distribusi awal.

2. Prediksi: Memperbarui setiap partikel berdasarkan model dinamika sistem.

3. Pengukuran: Menghitung bobot setiap partikel berdasarkan pengukuran sensor.

4. Resampling: Memilih ulang partikel berdasarkan bobotnya untuk mempertahankan partikel dengan estimasi yang lebih akurat.

Persamaan dasar dalam Particle Filter adalah:

• Prediksi:

  
  
  

• Update Bobot:

  
  
  

• Normalisasi Bobot: 

  
  
  

• Resampling: Menggunakan metode seperti sistem resampling berjenjang atau resampling sistematis untuk mempertahankan partikel yang lebih relevan.

Particle Filter sering digunakan dalam aplikasi seperti pelacakan objek, robotika, dan navigasi kendaraan otonom.

3. Implementasi dalam Python

3. Implementasi dalam Python

3.1 Implementasi Kalman Filter

import numpy as np
class KalmanFilter:
    def __init__(self, A, B, H, Q, R, P):
        self.A = A  # Matriks transisi keadaan
        self.B = B  # Matriks kontrol
        self.H = H  # Matriks observasi
        self.Q = Q  # Kovariansi proses
        self.R = R  # Kovariansi pengukuran
        self.P = P  # Kovariansi kesalahan
        self.x = np.zeros((A.shape[0], 1))
    
    def predict(self, u):
        self.x = np.dot(self.A, self.x) + np.dot(self.B, u)
        self.P = np.dot(np.dot(self.A, self.P), self.A.T) + self.Q
        return self.x
    
    def update(self, z):
        y = z - np.dot(self.H, self.x)
        S = np.dot(np.dot(self.H, self.P), self.H.T) + self.R
        K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
        self.x = self.x + np.dot(K, y)
        self.P = self.P - np.dot(np.dot(K, self.H), self.P)
        return self.x

3.2 Implementasi Complementary Filter

def complementary_filter(acc_angle, gyro_rate, alpha=0.98):
    return alpha * (gyro_rate) + (1 - alpha) * acc_angle

3.3 Implementasi Particle Filter

import random
def particle_filter(sensor_reading, particles, weights):
    new_particles = random.choices(particles, weights, k=len(particles))
    return new_particles

4. Kesimpulan

Sensor Fusion adalah teknik penting dalam sistem modern untuk menggabungkan berbagai data sensor secara optimal. Kalman Filter, Complementary Filter, dan Particle Filter adalah metode yang sering digunakan dalam berbagai aplikasi industri dan akademik.

Referensi

[1] Welch, G., & Bishop, G. (2006). An Introduction to the Kalman Filter. [2] Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. [3] Gustafsson, F. (2012). Statistical Sensor Fusion.