Matematika Estimasi Robotika — Blue Dragon Security

01

Konsep Dasar Estimasi State

Dalam robotika, state adalah kumpulan variabel yang cukup untuk mendeskripsikan kondisi sistem pada suatu waktu. Posisi robot (x, y, θ), kecepatan, orientasi, atau posisi sendi adalah contoh state.

// Definisi State Estimation

Estimasi state adalah proses menentukan nilai state sistem yang tidak dapat diukur secara langsung, menggunakan pengukuran sensor yang mengandung noise dan model dinamika sistem.

1.1 Masalah Fundamental

Dua sumber ketidakpastian utama:

Process noise (noise proses): ketidakpastian pada model dinamika, gangguan eksternal, dan error aktuator.
Measurement noise (noise pengukuran): ketidakpastian pada sensor, kalibrasi, dan lingkungan.

// Model Umum Sistem Diskrit x k = f (x k-1, u k) + w k // persamaan proses z k = h (x k) + v k // persamaan pengukuran // dimana: x k \in ℝⁿ \to state vektor pada waktu k u k \in ℝᵐ \to input kontrol z k \in ℝᵖ \to vektor pengukuran w k ~ N(0, Q) \to process noise, kovarians Q v k ~ N(0, R) \to measurement noise, kovarians R

1.2 Kriteria Estimator yang Baik

Kriteria	Definisi Matematis	Arti Praktis
Unbiased	E[x̂] = x	Rata-rata estimasi sama dengan nilai sesungguhnya
Consistent	lim P(\|x̂−x\|>ε) = 0	Error mengecil seiring bertambahnya data
Efficient (MMSE)	min E[\|\|x̂−x\|\|²]	Meminimalkan mean-squared error
MAP	argmax p(x\|z)	Memaksimalkan posterior probability

1.3 Distribusi Gaussian (Normal)

Filter Kalman dan banyak estimator bergantung pada asumsi distribusi Gaussian karena sifatnya yang tertutup terhadap operasi linier.

// Probability Density Function Gaussian p (x) = 1 / (\sqrt(2π| Σ |)) \cdot exp(-½ (x - μ)ᵀ Σ ⁻¹ (x - μ)) // Notasi ringkas: x ~ N (μ, Σ) μ = E[x] \to mean vektor (n \times 1) Σ = E[(x-μ)(x-μ)ᵀ] \to matriks kovarians (n \times n) | Σ | \to determinan Σ

// Properti Kunci Gaussian

Transformasi linier dari Gaussian menghasilkan Gaussian. Jika x ~ N(μ, Σ) dan y = Ax + b, maka y ~ N(Aμ + b, AΣAᵀ). Inilah mengapa Kalman Filter bekerja secara optimal untuk sistem linier-Gaussian.

02

Bayesian Estimation

Framework Bayesian adalah fondasi matematis untuk semua estimasi probabilistik. Seluruh filter (Kalman, Particle) adalah implementasi khusus dari Bayes filter.

2.1 Teorema Bayes

// Teorema Bayes p (x | z) = p (z | x) \cdot p (x) / p (z) // terminologi: p (x | z) \to posterior : probabilitas state x setelah observasi z p (z | x) \to likelihood : probabilitas pengukuran z jika state adalah x p (x) \to prior : kepercayaan awal sebelum pengukuran p (z) \to evidence : normalizing constant = \int p(z|x) p(x) dx

2.2 Bayes Filter (Recursive)

Bayes Filter memperbarui belief (distribusi probabilitas state) secara rekursif dalam dua langkah.

ALGORITMA Bayes Filter Rekursif

Input: bel(x_k−1), u_k, z_k Output: bel(x_k) STEP 1 PREDICTION (Chapman-Kolmogorov): b̄el(x_k) = ∫ p(x_k | x_k−1, u_k) · bel(x_k−1) dx_k−1 STEP 2 UPDATE (Bayes): bel(x_k) = η · p(z_k | x_k) · b̄el(x_k) η = normalizing constant agar ∫ bel(x_k) dx = 1

2.3 MAP vs MMSE

Estimator	Formula	Keterangan
MAP (Maximum A Posteriori)	x̂_MAP = argmax p(x\|z)	Mode dari distribusi posterior. Robust terhadap outlier
MMSE (Min Mean Sq Error)	x̂_MMSE = E[x\|z] = ∫ x p(x\|z) dx	Mean dari posterior. Optimal untuk Gaussian
ML (Maximum Likelihood)	x̂_ML = argmax p(z\|x)	Tidak menggunakan prior. Tidak cocok jika prior informative

// Catatan Penting

Untuk distribusi Gaussian, MAP = MMSE = Mean posterior. Ini adalah alasan kuat mengapa Kalman Filter yang mengasumsikan Gaussian adalah estimator optimal (MMSE) untuk sistem linier.

03

Representasi State Space

State space representation adalah cara matematis untuk mendeskripsikan sistem dinamis menggunakan variabel state internal.

3.1 Model Diskrit-Waktu Linier

// State Space Linier Diskrit x k = F x k-1 + B u k + w k z k = H x k + v k // matriks sistem: F \in ℝⁿˣⁿ \to state transition matrix B \in ℝⁿˣᵐ \to control input matrix H \in ℝᵖˣⁿ \to observation (measurement) matrix Q \in ℝⁿˣⁿ \to process noise covariance (simetrik, PSD) R \in ℝᵖˣᵖ \to measurement noise covariance (simetrik, PD)

3.2 Contoh: Model Gerak Robot 2D

Robot bergerak pada bidang datar. State: posisi dan kecepatan pada sumbu x dan y.

// State Vektor & Matriks Transisi x k = [x, ẋ, y, ẏ]ᵀ // 4-dimensi state // F dengan Δt = interval waktu sampling: F = ⎡ 1 Δt 0 0 ⎤ ⎢ 0 1 0 0 ⎥ ⎢ 0 0 1 Δt ⎥ ⎣ 0 0 0 1 ⎦ // Jika sensor hanya mengukur posisi (bukan kecepatan): H = ⎡ 1 0 0 0 ⎤ ⎣ 0 0 1 0 ⎦ // 2\times4, observasi x dan y saja

3.3 Model Non-Linier Umum

Kebanyakan robot menggunakan model non-linier karena kinematika dan sensor yang melibatkan fungsi trigonometri.

// Model Non-Linier (EKF/UKF/Particle) x k = f (x k-1, u k) + w k z k = h (x k) + v k // Contoh: unicycle robot model f (x, u) = ⎡ x + v\cdotcos(θ)\cdotΔt ⎤ ⎢ y + v\cdotsin(θ)\cdotΔt ⎥ ⎣ θ + ω\cdotΔt ⎦ // dimana u = [v, ω]ᵀ (linear speed, angular speed) // Contoh: sensor range-bearing h (x) = ⎡ \sqrt((mx-x)² + (my-y)²) ⎤ // jarak ke landmark ⎣ atan2(my-y, mx-x) - θ ⎦ // sudut ke landmark

04

Kalman Filter (KF)

Kalman Filter (1960) adalah estimator optimal rekursif untuk sistem linier dengan noise Gaussian. Memberikan estimasi minimum variance unbiased (MVUE).

// Asumsi Kalman Filter

1. Model sistem linier: x_k = Fx_k−1 + Bu_k + w_k
2. Noise Gaussian putih: w_k ~ N(0,Q), v_k ~ N(0,R)
3. Noise proses dan pengukuran tidak berkorelasi: E[w_kv_jᵀ] = 0
4. Prior Gaussian: x₀ ~ N(μ₀, P₀)

4.1 Model Sistem Kalman Filter

// Definisi Lengkap Model KF // === PERSAMAAN PROSES === x k = F k x k-1 + B k u k + w k // === PERSAMAAN PENGUKURAN === z k = H k x k + v k // === STATISTIK NOISE === E[w k] = 0 E[w k w j ᵀ] = Q k δ kj // δ = Kronecker delta E[v k] = 0 E[v k v j ᵀ] = R k δ kj E[w k v j ᵀ] = 0 \forallk,j

4.2 Algoritma: Predict & Update

ALGORITMA Kalman Filter — 5 Persamaan

INISIALISASI: x̂_0|0 = μ₀ // estimasi awal state P_0|0 = P₀ // kovarians error awal ── FASE PREDICT ────────────────────────── 1 State prediction: x̂_k|k−1 = F_k x̂_k−1|k−1 + B_k u_k 2 Covariance prediction: P_k|k−1 = F_k P_k−1|k−1 F_kᵀ + Q_k ── FASE UPDATE ─────────────────────────── 3 Innovation (residual): ỹ_k = z_k − H_k x̂_k|k−1 // measurement residual 4 Innovation covariance: S_k = H_k P_k|k−1 H_kᵀ + R_k 5 Kalman Gain: K_k = P_k|k−1 H_kᵀ S_k⁻¹ 6 State update: x̂_k|k = x̂_k|k−1 + K_k ỹ_k 7 Covariance update (Joseph form): P_k|k = (I − K_k H_k) P_k|k−1 (I − K_k H_k)ᵀ + K_k R_k K_kᵀ // Joseph form: numerically stable, selalu PSD // Simplified: P_k|k = (I − K_k H_k) P_k|k−1 (hanya valid jika K optimal)

4.3 Interpretasi Kalman Gain

// Analisis Kalman Gain K K k = P k|k-1 H k ᵀ (H k P k|k-1 H k ᵀ + R k)⁻¹ // Case 1: Sensor sangat akurat \to R \to 0 K \to H ⁻¹ // filter percaya penuh pada sensor x̂ k|k \approx H ⁻¹ z k // langsung pakai pengukuran // Case 2: Model sangat akurat \to P \to 0 K \to 0 // filter percaya penuh pada prediksi x̂ k|k \approx x̂ k|k-1 // abaikan pengukuran // Kenyataannya: K adalah weighted trade-off optimal antara keduanya

// Intuisi Kalman Gain

Kalman Gain adalah bobot yang menyeimbangkan kepercayaan antara prediksi model dan pengukuran sensor. Semakin besar P (ketidakpastian prediksi) relatif terhadap R (ketidakpastian sensor), semakin besar bobot diberikan ke sensor.

4.4 Contoh Numerik: 1D Position Tracking

// KF 1 Dimensi — Robot Melacak Posisi // State: x = [posisi, kecepatan]ᵀ, Δt = 0.1s F = [[1, 0.1], [0, 1]] H = [[1, 0]] // hanya posisi diukur Q = [[0.001, 0],[0, 0.001]] R = [[0.5]] // sensor GPS noise σ²=0.5m² // Iterasi k=1, x̂₀ = [0,0]ᵀ, P₀ = I\cdot10 // PREDICT: x̂ ₁|₀ = [[1,0.1],[0,1]] \cdot [0,0]ᵀ = [0, 0]ᵀ P ₁|₀ = F\cdotP₀\cdotFᵀ + Q \approx [[10.001, 1.0], [1.0, 10.001]] // Misalkan z₁ = 0.5m (GPS ukur 0.5m) // UPDATE: S = H\cdotP₁|₀\cdotHᵀ + R = 10.001 + 0.5 = 10.501 K = P₁|₀\cdotHᵀ/S = [10.001/10.501, 1.0/10.501]ᵀ \approx [0.952, 0.095]ᵀ x̂ ₁|₁ = [0,0]ᵀ + K\cdot(0.5-0) = [0.476, 0.048]ᵀ // Estimasi posisi: 0.476m (antara prediksi 0 dan sensor 0.5)

05

Extended Kalman Filter (EKF)

EKF adalah ekstensi Kalman Filter untuk sistem non-linier dengan melakukan linearisasi orde pertama menggunakan Jacobian.

// Keterbatasan KF → Motivasi EKF

Robot nyata menggunakan model kinematika non-linier (unicycle, differential drive, arm robot). Sensor seperti LIDAR dan kamera menghasilkan data yang hubungannya non-linier dengan state. KF standar tidak bisa menangani ini.

5.1 Linearisasi dengan Jacobian

// Taylor Expansion Orde Pertama // Fungsi non-linier: x k = f (x k-1, u k) + w k z k = h (x k) + v k // Linearisasi sekitar estimasi saat ini x̂: f (x) \approx f (x̂) + F (x - x̂) // F adalah Jacobian df/dx h (x) \approx h (x̂) + H (x - x̂) // H adalah Jacobian dh/dx // Jacobian matriks (partial derivatives): F k = \partial f /\partial x | x=x̂ k-1|k-1 // n\timesn H k = \partial h /\partial x | x=x̂ k|k-1 // p\timesn

5.2 Algoritma EKF

ALGORITMA Extended Kalman Filter

── FASE PREDICT ────────────────────────── 1 State prediction (non-linier): x̂_k|k−1 = f(x̂_k−1|k−1, u_k) 2 Hitung Jacobian F_k: F_k = ∂f/∂x |_{x̂_k−1|k−1} 3 Covariance prediction (linearisasi): P_k|k−1 = F_k P_k−1|k−1 F_kᵀ + Q_k ── FASE UPDATE ─────────────────────────── 4 Hitung Jacobian H_k: H_k = ∂h/∂x |_{x̂_k|k−1} 5 Innovation: ỹ_k = z_k − h(x̂_k|k−1) // gunakan h non-linier di sini 6 Innovation covariance: S_k = H_k P_k|k−1 H_kᵀ + R_k 7 Kalman Gain: K_k = P_k|k−1 H_kᵀ S_k⁻¹ 8 State update: x̂_k|k = x̂_k|k−1 + K_k ỹ_k 9 Covariance update: P_k|k = (I − K_k H_k) P_k|k−1

5.3 Contoh: Jacobian Unicycle Model

// Derivasi Jacobian untuk Unicycle Robot // Model proses: f(x, u) dengan x=[x,y,θ]ᵀ, u=[v,ω]ᵀ f ([x,y,θ], [v,ω]) = ⎡ x + v\cdotcos(θ)\cdotΔt ⎤ ⎢ y + v\cdotsin(θ)\cdotΔt ⎥ ⎣ θ + ω\cdotΔt ⎦ // Jacobian F = \partialf/\partialx : F = \partialf/\partial[x,y,θ] = ⎡ 1 0 -v\cdotsin(θ)\cdotΔt ⎤ ⎢ 0 1 v\cdotcos(θ)\cdotΔt ⎥ ⎣ 0 0 1 ⎦ // Model sensor bearing-only: h(x) = atan2(my-y, mx-x) - θ // Jacobian H = \partialh/\partialx : dx = mx - x, dy = my - y, q = dx² + dy² H = [dy /q, - dx /q, -1 ] // 1\times3 row vector

// Kelemahan EKF

EKF menggunakan linearisasi orde pertama yang bisa mengakumulasi error saat non-linearitas kuat. Divergensi filter dapat terjadi jika inisialisasi buruk atau kovarians tidak akurat. Untuk non-linearitas tinggi, gunakan UKF atau Particle Filter.

06

Unscented Kalman Filter (UKF)

UKF menggunakan Unscented Transform (UT) untuk mempropagasi distribusi melalui fungsi non-linier menggunakan kumpulan titik deterministik (sigma points) tanpa menghitung Jacobian.

6.1 Unscented Transform

// Sigma Point Generation — UT // Diberikan: distribusi x ~ N(x̂, P), dimensi n // Parameter: α, β, κ (biasanya α=1e-3, β=2, κ=0) λ = α² (n + κ) - n // 2n+1 sigma points: χ ⁰ = x̂ χ ⁱ = x̂ + (\sqrt((n+ λ)P)) i i = 1, ..., n χ ⁱ = x̂ - (\sqrt((n+ λ)P)) i-n i = n+1, ..., 2n // (\sqrt((n+λ)P)) i adalah kolom ke-i dari matrix square root // Bobot untuk mean dan covariance: W m ⁰ = λ /(n+ λ) W c ⁰ = λ /(n+ λ) + (1 - α² + β) W m ⁱ = W c ⁱ = 1 / (2(n+ λ)) i = 1, ..., 2n

6.2 Algoritma UKF Lengkap

ALGORITMA Unscented Kalman Filter

── FASE PREDICT ────────────────────────── 1 Generate sigma points: χ_k−1 = SigmaPoints(x̂_k−1|k−1, P_k−1|k−1) 2 Propagasi sigma points (non-linier): χ*_k|k−1ⁱ = f(χ_k−1ⁱ, u_k) untuk i = 0,...,2n 3 Predicted mean: x̂_k|k−1 = Σ W_mⁱ · χ*_k|k−1ⁱ 4 Predicted covariance: P_k|k−1 = Σ W_cⁱ (χ*ⁱ−x̂)(χ*ⁱ−x̂)ᵀ + Q_k ── FASE UPDATE ─────────────────────────── 5 Propagasi sigma points ke observation space: γ_kⁱ = h(χ*_k|k−1ⁱ) 6 Predicted measurement mean: ẑ_k = Σ W_mⁱ · γ_kⁱ 7 Innovation covariance S & cross-covariance C: S_k = Σ W_cⁱ (γⁱ−ẑ)(γⁱ−ẑ)ᵀ + R_k C_k = Σ W_cⁱ (χ*ⁱ−x̂_k|k−1)(γⁱ−ẑ)ᵀ 8 Kalman Gain: K_k = C_k S_k⁻¹ 9 State & Covariance update: x̂_k|k = x̂_k|k−1 + K_k (z_k − ẑ_k) P_k|k = P_k|k−1 − K_k S_k K_kᵀ

6.3 Perbandingan KF vs EKF vs UKF

Aspek	KF	EKF	UKF
Model Sistem	Linier	Non-linier	Non-linier
Pendekatan	Eksak	Linearisasi Jacobian	Sigma Points (UT)
Butuh Jacobian	Tidak	Ya	Tidak
Akurasi	Optimal (linier)	Orde 1	Orde 3 (lebih baik)
Komputasi	O(n²)	O(n²) + Jacobian	O(n²) sigma points
Mudah Implementasi	Tinggi	Sedang (perlu derivatif)	Tinggi (no derivatif)

07

Particle Filter (Sequential Monte Carlo)

Particle Filter merepresentasikan distribusi posterior sebagai kumpulan sampel berbobot (partikel). Dapat menangani distribusi multi-modal dan model non-Gaussian secara penuh.

7.1 Konsep Dasar

// Representasi Particle // Posterior diaproksimasikan oleh N partikel: p (x k | z 1:k) \approx Σ i=1..N w k ⁱ \cdot δ(x - x k ⁱ) // Set partikel: {x k ⁱ, w k ⁱ} i=1..N x k ⁱ \to sampel state partikel ke-i w k ⁱ \to bobot partikel ke-i // Normalisasi: Σ w k ⁱ = 1 // Estimasi mean: x̂ k = Σ i w k ⁱ x k ⁱ

7.2 Sequential Importance Sampling (SIS)

// Importance Sampling Weights Update // Proposal distribution: q(x k |x k-1, z k) // Bobot diperbarui secara sekuensial: w k ⁱ \propto w k-1 ⁱ \cdot p (z k | x k ⁱ) \cdot p (x k ⁱ| x k-1 ⁱ) ────────────────────────────────── q(x k ⁱ | x k-1 ⁱ, z k) // Pilihan umum: q = prior transition \to proposal = p(x k |x k-1) // Sederhananya: w k ⁱ \propto w k-1 ⁱ \cdot p(z k |x k ⁱ)

7.3 Algoritma SIR Particle Filter

ALGORITMA SIR Particle Filter (Bootstrap)

INISIALISASI (k=0): Sampel x₀ⁱ ~ p(x₀), set w₀ⁱ = 1/N ∀i FOR k = 1, 2, ... DO: 1 PREDICT — Sampel dari prior: x̃_kⁱ ~ p(x_k | x_k−1ⁱ, u_k) i=1..N // tambahkan process noise: x̃ = f(x_prev, u) + sample(Q) 2 WEIGHT — Update bobot dengan likelihood: w̃_kⁱ = w_k−1ⁱ · p(z_k | x̃_kⁱ) // likelihood sensor: e.g. Gaussian p(z|x) = N(z; h(x), R) 3 NORMALIZE: w_kⁱ = w̃_kⁱ / Σ_j w̃_kʲ 4 ESTIMATE: x̂_k = Σ_i w_kⁱ · x̃_kⁱ 5 RESAMPLE — jika N_eff < threshold: N_eff = 1 / Σ_i (w_kⁱ)² // Low N_eff = degeneracy (partikel terpusat di 1 titik) // Resample N partikel dari {x̃ⁱ, wⁱ} → {xⁱ, 1/N} // Gunakan systematic resampling (paling efisien) END FOR

7.4 Systematic Resampling

// Systematic Resampling Algorithm // Buat N partikel baru dari distribusi {xⁱ, wⁱ}: U ~ Uniform(0, 1/N) // satu sampel acak c[0] = w¹, c[i] = c[i-1] + w i+1 // CDF kumulatif j = 1 FOR i = 1 TO N: u = U + (i-1)/N WHILE c[j] < u : j++ x new ⁱ = x old ʲ // O(N) waktu — lebih baik dari multinomial O(N log N)

// Trade-off: Jumlah Partikel N

Makin banyak partikel → akurasi lebih tinggi → komputasi lebih berat. Untuk robot 2D (x,y,θ): N=500–2000 cukup. Untuk high-dimensional state atau multi-modal, N bisa mencapai 10.000+. Adaptive particle filter dapat menyesuaikan N secara dinamis.

08

Sensor Fusion

Sensor Fusion menggabungkan data dari beberapa sensor untuk menghasilkan estimasi state yang lebih akurat daripada menggunakan satu sensor saja.

8.1 Prinsip Dasar

// Optimal Fusion: 2 Sensor (1D) // Dua pengukuran independen dari state yang sama: z ₁ ~ N(x, σ₁²) // sensor 1 z ₂ ~ N(x, σ₂²) // sensor 2 // Fusi optimal (MMSE): x̂ = (σ₂² \cdot z ₁ + σ₁² \cdot z ₂) / (σ₁² + σ₂²) σ² = (σ₁² \cdot σ₂²) / (σ₁² + σ₂²) // kovarians fusi // Atau dalam bentuk bobot: x̂ = w₁\cdot z ₁ + w₂\cdot z ₂ w₁ = σ₂²/(σ₁²+σ₂²), w₂ = σ₁²/(σ₁²+σ₂²) // Sensor yang lebih akurat (σ kecil) mendapat bobot lebih besar

8.2 Fusi IMU + GPS menggunakan EKF

// IMU + GPS Fusion — State Space // State: posisi, kecepatan, orientasi, IMU bias x = [p, v, q, b a, b g]ᵀ \in ℝ¹⁶ // p \in ℝ³: posisi, v \in ℝ³: kecepatan, q \in ℝ⁴: quaternion // b a \in ℝ³: acc bias, b g \in ℝ³: gyro bias // Predict menggunakan IMU pre-integration: p k = p k-1 + v k-1 Δt + ½(R (ã - b a) - g)Δt² v k = v k-1 + (R (ã - b a) - g)Δt q k = q k-1 \otimes Exp ((ω̃ - b g)Δt) // quaternion multiplication // Update menggunakan GPS (z_GPS = [lat,lon,alt] \to xyz): z GPS = p + v GPS \to H GPS = [I₃, 0, 0, 0, 0]

8.3 Complementary Filter (IMU + Magnetometer)

// Complementary Filter — Estimasi Orientasi // Prinsip: IMU bagus short-term (drift), Mag bagus long-term // Complementary filter gabungkan keduanya di frekuensi berbeda: θ̂ k = α \cdot (θ̂ k-1 + ω gyro \cdotΔt) + (1-α) \cdot θ acc/mag // α \approx 0.98 (lebih percaya gyro, filter high-pass) // θ acc/mag : estimasi statis dari accelerometer + magnetometer // (1-α) = 0.02: filter low-pass untuk drift correction // Transfer function: H gyro (s) = αs/(αs+1) // high-pass filter H acc (s) = (1-α)/(αs+1) // low-pass filter // Sum = 1 (complementary!)

8.4 Diagram Arsitektur Sensor Fusion

09

Odometri & Pengantar SLAM

9.1 Odometri Differential Drive

Odometri mengintegrasi data encoder roda untuk mengestimasi pose robot. Akumulasi error tanpa koreksi eksternal — ini masalah utama yang diselesaikan SLAM.

// Kinematik Differential Drive // Encoder kiri dan kanan memberikan jarak roda: Δs_L = (2π r / N ticks) \cdot n_L Δs_R = (2π r / N ticks) \cdot n_R // Jarak dan rotasi rata-rata: Δs = (Δs_R + Δs_L) / 2 // jarak maju rata-rata Δθ = (Δs_R - Δs_L) / L // L = jarak antar roda (wheelbase) // Update pose: x' = x + Δs \cdot cos(θ + Δθ/2) y' = y + Δs \cdot sin(θ + Δθ/2) θ' = θ + Δθ // Kovarians propagasi (linearisasi): P' = J pose \cdot P \cdot J pose ᵀ + J enc \cdot Q enc \cdot J enc ᵀ // Q enc = diag(k_r|Δs_R|, k_l|Δs_L|) (linear dalam jarak)

9.2 Error Odometri dan Akumulasi

Sumber Error	Model Matematis	Efek
Slip roda	σ²_slip ∝ \|Δs\|	Error posisi terakumulasi √t
Ketidakseimbangan diameter roda	δr = r_R − r_L ≠ 0	Robot tidak jalan lurus, heading drift linier
Resolusi encoder	σ_enc = 2πr/(2N)	Kuantisasi error per step
Error heading	σ²_xy ≈ (d·σ_θ)²	Tumbuh kuadratik terhadap jarak d

9.3 EKF-SLAM: Konsep Matematika

// SLAM State Vector // State gabungan: pose robot + peta landmark X = [x r, m ₁, m ₂, ..., m N]ᵀ \in ℝ³⁺²ᴺ x r = [x, y, θ]ᵀ // pose robot m i = [m_xi, m_yi]ᵀ // posisi landmark ke-i // Kovarians gabungan: (3+2N) \times (3+2N) P = ⎡ P rr P rm ⎤ ⎣ P mr P mm ⎦ P rr : kovarians pose robot P mm : kovarians landmark P rm : cross-korelasi (penting!) robot-landmark // Seiring waktu, landmark dan pose menjadi sangat berkorelasi // Ini yang membuat loop-closure bekerja dalam SLAM

ALGORITMA EKF-SLAM Satu Iterasi

1 PREDICT — hanya robot bergerak, landmark tetap: x̂'_r = f(x̂_r, u_k) P' = F P Fᵀ + Q // F = blok-diag([F_r, I_2N]) — landmark tidak berubah 2 DATA ASSOCIATION — cocokkan pengamatan ke landmark: d²_ij = (z_i − ẑ_j)ᵀ S⁻¹ (z_i − ẑ_j) Mahalanobis distance // nearest-neighbour atau Hungarian algorithm 3 UPDATE untuk setiap landmark yang terdeteksi: H = ∂h/∂X |_X̂ K = P Hᵀ (H P Hᵀ + R)⁻¹ X̂ += K (z − h(X̂)) P = (I − KH) P 4 ADD NEW LANDMARKS (landmark baru terdeteksi): // Augmentasi state vector: tambah [m_x, m_y] // Inisialisasi kovarians via Jacobian inverse observation

9.4 Perbandingan Algoritma SLAM

Algoritma	Representasi	Kompleksitas	Keunggulan
EKF-SLAM	Gaussian	O(N²) per step	Optimal untuk linearisasi, kovarians penuh
FastSLAM	Partikel + EKF per-landmark	O(N log M)	Skalabel, multi-hypothesis
Graph-SLAM / iSAM	Factor graph	O(N log N)	Loop closure yang kuat, batch optimization
ORB-SLAM3	Feature-based visual	Bergantung fitur	Real-time, stereo+IMU, multi-session

REF

Notasi dan Simbol Referensi

Simbol	Nama	Dimensi	Keterangan
x_k	State vector	n×1	State sistem pada waktu k
u_k	Control input	m×1	Sinyal kontrol atau aktuasi
z_k	Measurement vector	p×1	Keluaran sensor pada waktu k
F	State transition matrix	n×n	Linearisasi fungsi f
H	Observation matrix	p×n	Linearisasi fungsi h
Q	Process noise cov.	n×n	Simetrik PSD
R	Measurement noise cov.	p×p	Simetrik PD
P	Error covariance	n×n	Ketidakpastian estimasi
K	Kalman Gain	n×p	Bobot optimal antara prediksi dan sensor
S	Innovation covariance	p×p	S = HPHᵀ + R
ỹ	Innovation / residual	p×1	Perbedaan pengukuran dengan prediksi
x̂_k\|k	Posterior estimate	n×1	Setelah update pengukuran k
x̂_k\|k−1	Prior estimate	n×1	Setelah predict, sebelum update
χ	Sigma points (UKF)	n×(2n+1)	Titik deterministik UT
W_m, W_c	UT weights	2n+1	Bobot mean dan kovarians
{xⁱ, wⁱ}	Particle set	N sampel	Partikel dan bobotnya (PF)
N_eff	Effective sample size	skalar	1/Σ(wⁱ)², trigger resample

Referensi Utama

Thrun, Burgard, Fox — Probabilistic Robotics (2005, MIT Press) — Referensi utama SLAM dan Bayes filter
Kalman, R.E. — A New Approach to Linear Filtering and Prediction Problems (1960, ASME Trans.)
Wan & van der Merwe — The Unscented Kalman Filter, Kalman Filtering and Neural Networks (2001)
Doucet, de Freitas, Gordon — Sequential Monte Carlo Methods in Practice (2001, Springer)
Bar-Shalom, Li, Kirubarajan — Estimation with Applications to Tracking and Navigation (2001, Wiley)
Barfoot — State Estimation for Robotics (2017, Cambridge University Press)