0% found this document useful (0 votes)
36 views5 pages

Sparse Kalman Filter: 1 Prediction-Type Operations

1. The document summarizes the key operations of the sparse Kalman filter, including prediction, initialization, reparametrization, and correction for both individual and buffered landmarks. 2. It outlines the general and sparse functions, inputs/outputs, and matrix partitions for each operation. 3. The Jacobian, covariance, expectation, innovation, and Kalman gain matrices are defined for computing the sparse Kalman filter updates.

Uploaded by

Alexgri
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
0% found this document useful (0 votes)
36 views5 pages

Sparse Kalman Filter: 1 Prediction-Type Operations

1. The document summarizes the key operations of the sparse Kalman filter, including prediction, initialization, reparametrization, and correction for both individual and buffered landmarks. 2. It outlines the general and sparse functions, inputs/outputs, and matrix partitions for each operation. 3. The Jacobian, covariance, expectation, innovation, and Kalman gain matrices are defined for computing the sparse Kalman filter updates.

Uploaded by

Alexgri
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 5

Sparse Kalman Filter

Joan Solà

April 23, 2010

1 Prediction-type operations
1.1 Robot motion
General function x = f (x, u, q)
Sparse function r = f (r, u, q)
In r robot
Invariant m mapped landmarks
Out r robot
Ignored in i
Used in x=r+m
Used out x=r+m
Ignored out i

Matrix partitions  
x= r m i
Jacobian  
Fr 0 ∗
F =  0 I ∗
∗ ∗ ∗
Covariances  
Prr Prm ∗
P = Pmr Pmm ∗
∗ ∗ ∗
Output covariances

Fr Prr Fr> + Qrr Fr Prm ∗


 

FPF> + Q =  Pmr Fr> Pmm ∗


∗ ∗ ∗

1
1.2 Landmark initialization
General function x = g(x, y, n)
Sparse function l = g(r, y, n)
In r robot+sensor
Invariant r+m robot+sensor+map
Out l new landmark
Ignored in i
Used in x=r+m
Used out x=r+m+l
Ignored out i−l

Matrix partitions  
x= r m l i
Jacobian  
I 0 ∗ ∗
0 I ∗ ∗
G= 
Gr 0 ∗ ∗
∗ ∗ ∗ ∗
Covariances  
Prr Prm ∗ ∗
Pmr Pmm ∗ ∗
P =
 ∗

∗ ∗ ∗
∗ ∗ ∗ ∗
Output covariances

Prr G>
 
Prr Prm r ∗
P P P G > ∗
mr mm mr
GP G> + (R) + (N ) = 
 r
Gr Prr Gr Prm Gr Prr Gr + Gy RG>
> >

y + Gn N Gn ∗
∗ ∗ ∗ ∗

2
1.3 Landmark re-parametrization
General function x = j(x)
Sparse function l = j(k)
In k old landmark
Invariant m all map
Out l new landmark
Ignored in i
Used in x=k+m map with old lmk
Used out x=m+l map with new lmk
Ignored out i+k−l

Matrix partitions  
x= k m l i
Jacobian  
∗ ∗ ∗ ∗
0 I ∗ ∗
J = 
Jk 0 ∗ ∗
∗ ∗ ∗ ∗
Covariances  
Pkk Pkm ∗ ∗
Pmk Pmm ∗ ∗
P =
 ∗

∗ ∗ ∗
∗ ∗ ∗ ∗
Output covariances  
∗ ∗ ∗ ∗
∗ Pmm P mk Jk> ∗
JP J > =
∗ Jk Pkm Jk Pkk J >

k ∗
∗ ∗ ∗ ∗

3
2 Correction-type operations
2.1 Individual landmark correction
General function y = h(x)
Sparse function y = h(r, l)
In 1 r robot+sensor
In 2 l observed landmark
Passive m other landmarks
Out y measurement
Updated r+m+l robot+sensor+landmarks
Ignored i

Matrix partitions  
x= k m l i
Jacobian  
H = Hr 0 Hl ∗
Covariances  
Prr Prm Prl ∗
Pmr Pmm Pml ∗
P = 
 Plr Plm Pll ∗
∗ ∗ ∗ ∗
Expectation matrix

E = Hr Prr Hr> + Hr Prl Hl> + Hl Plr Hr> + Hl Pll Hl>

Innovation matrix
Z =E+R
Band matrix
Pxr Hr> + Pxl Hl>
 
P H> =

Kalman gain
K = P H > Z −1
Covariances update
P = P − K(P H > )>

4
2.2 Buffered landmarks correction
General function y = h(x)
Sparse function y = h(r, l1 , l2 )
In 1 r robot+sensor
In 2 l1 observed landmark
In 3 l2 observed landmark
Passive m other landmarks
Out y1 measurement
Out y2 measurement
Updated r + m + l1 + l2 robot + sensor + all landmarks
Ignored i

Matrix partitions  
x = r m l1 l2 i
Jacobian  
Hr1 0 Hl1 0 ∗
H=
Hr2 0 0 Hl2 ∗
Covariances  
Prr Prm Pr1 Pr2 ∗
Pmr Pmm Pm1 Pm2
 ∗

 P1r P1m P11 P12
P = ∗

 P2r P2m P21 P22 ∗
∗ ∗ ∗ ∗ ∗
Expectation matrix  
E11 E12
E=
E21 E22
with
>
Eij = Hri Prr Hrj >
+ Hri Prj Hlj> + Hli Pij Hlj> + Hli Pir Hrj
Innovation matrix  
E11 + R E12
Z=
E21 E22 + R
Band matrix
P H> (P H > )1 (P H > )2
   
=
∗ ∗ ∗
with
(P H > )i = Pxr Hri
> + P H>
 
xi li

Kalman gain
P H > −1
   
K
= Z
∗ ∗
Covariances update
P = P − K(P H > )>

You might also like