Probabilistic active filtering with gaussian processes for occluded object search in clutter

Abstract

This paper proposes a Gaussian process model-based probabilistic active learning approach for occluded object search in clutter. Due to heavy occlusions, an agent must be able to gradually reduce uncertainty during the observations of objects in its workspace by systematically rearranging them. In this work, we apply a Gaussian process to capture the uncertainties of both system dynamics and observation function. Robot manipulation is optimized by mutual information that naturally indicates the potential of moving one object to search for new objects based on the predicted uncertainties of two models. An active learning framework updates the state belief based on sensor observations. We validated our proposed method in a simulation robot task. The results demonstrate that with samples generated by random actions, the proposed method can learn intelligent object search behaviors while iteratively converging its predicted state to the ground truth.

Appendix A

1.1 A.1 Analytical moment matching of observation function

Consider the exact analytical expression of the observation function in (11):

$$ h_{g}(\boldsymbol{\mu}, \boldsymbol{\Sigma}) \approx \int p\left( GP_{g}(\boldsymbol{x}_{*})|\boldsymbol{x}_{*}\right)p(\boldsymbol{x}_{*}|\boldsymbol{\mu}, \boldsymbol{\Sigma})\mathrm{d} \boldsymbol{x}_{*}. $$

(16)

Following [25, 26], the mean and variance of h_g(μ,Σ) in each dimension a = 1,...,D are calculated:

$$ \mu_{g_{a}} = \boldsymbol{\upbeta}_{g_{a}}^{\top}\int \boldsymbol{k}_{g_{a}}(\boldsymbol{x}_{*}) p(\boldsymbol{x}_{*}|\boldsymbol{\mu}, \boldsymbol{\Sigma}) \mathrm{d} \boldsymbol{x}_{*} = \boldsymbol{\upbeta}_{g_{a}}^{\top}\boldsymbol{l}_{g_{a}}, $$

(17)

where $\boldsymbol {\upbeta }_{g_{a}}=(\boldsymbol {K}^{g_{a}}+\alpha ^{2}_{g_{a}}\boldsymbol {I})^{-1}\boldsymbol {Z}^{a}$. For target dimension a,b = 1,...,D and a≠b, predicted variance Σ_aa and covariance Σ_ab of h(μ,Σ) follow:

$$ \begin{array}{@{}rcl@{}} {\Sigma}_{g_{aa}}&=& \mathbb{E}\left[\sigma_{g_{a}}^{2}(\boldsymbol{x})\right] + \mathbb{E}\left[m_{g_{a}}^{2}(\boldsymbol{x})\right] - \mu_{g_{a}}^{2} \\&=& \boldsymbol{\upbeta}_{g_{a}}^{\top}\boldsymbol{L}^{g}\boldsymbol{\upbeta}_{g_{a}} + \alpha^{2}_{g_{a}} - tr\left( (\boldsymbol{K}^{g_{a}} + \sigma_{\epsilon_{a}}^{2}\boldsymbol{I})^{-1}\boldsymbol{L}^{g}\right) - \mu_{g_{a}}^{2},\\ {\Sigma}_{g_{ab}} &=& \mathbb{E}\left[m_{g_{a}}(\boldsymbol{x})m_{g_{b}}(\boldsymbol{x})\right]- \mu_{g_{a}}\mu_{g_{b}} = \boldsymbol{\upbeta}_{g_{a}}^{\top}\boldsymbol{Q}^{g}\boldsymbol{\upbeta}_{g_{b}} - \mu_{g_{a}}\mu_{g_{b}}. \end{array} $$

(18)

Vectors $\boldsymbol {l}_{g_{a}}$ and matrices L^g, Q^g have the following elements:

$$ \begin{array}{@{}rcl@{}} l_{g_{ai}} &=& \int k_{g_{a}}(\boldsymbol{x}_{i}, \boldsymbol{x}_{*}) p(\boldsymbol{x}_{*}|\boldsymbol{\mu}, \boldsymbol{\Sigma}) \mathrm{d} \boldsymbol{x}_{*}\\ &=&\alpha^{2}_{g_{a}}|\boldsymbol{\Sigma}\boldsymbol{\varLambda}_{g_{a}}^{-1} + \boldsymbol{I}|^{-\frac{1}{2}}\\ &&\times\exp\left( -\frac{1}{2}(\boldsymbol{x}_{i}-\boldsymbol{\mu})^{\top}(\boldsymbol{\Sigma}+\boldsymbol{\varLambda}_{g_{a}})^{-1}(\boldsymbol{x}_{i}-\boldsymbol{\mu})\right). \end{array} $$

(19)

$$ \begin{array}{@{}rcl@{}} L_{ij}^{g}& =& \frac{k_{g_{a}}(\boldsymbol{x}_{i},\boldsymbol{\mu})k_{g_{a}}(\boldsymbol{x}_{j},\boldsymbol{\mu})}{|2\boldsymbol{\Sigma}\boldsymbol{\varLambda}_{g_{a}}^{-1} + \boldsymbol{I}|^{\frac{1}{2}}}\\ &&\times \exp\left( (\boldsymbol{z}_{ij}-\boldsymbol{\mu})^{\top}(\boldsymbol{\Sigma}+\frac{1}{2}\boldsymbol{\varLambda}_{g_{a}})^{-1}\boldsymbol{\Sigma}\boldsymbol{\varLambda}_{ga}^{-1}(\boldsymbol{z}_{ij}-\boldsymbol{\mu})\right), \end{array} $$

(20)

$$ \begin{array}{@{}rcl@{}} Q_{ij}^{g} &=& \alpha_{g_{a}}^{2}\alpha_{g_{b}}^{2}|(\boldsymbol{\varLambda}_{g_{a}}^{-1}+\boldsymbol{\varLambda}_{g_{b}}^{-1})\boldsymbol{\Sigma}+\boldsymbol{I}|^{-\frac{1}{2}}\\ &\times&\exp\left( -\frac{1}{2}(\boldsymbol{x}_{i} - \boldsymbol{x}_{j})^{\top}(\boldsymbol{\varLambda}_{g_{a}} + \boldsymbol{\varLambda}_{g_{b}})^{-1}(\boldsymbol{x}_{i} - \boldsymbol{x}_{j})\right)\\ &\times&\exp\left( -\frac{1}{2}(\boldsymbol{z}_{ij}^{\prime}-\boldsymbol{\mu})^{\top}\boldsymbol{R}^{-1}(\boldsymbol{z}_{ij}^{\prime}-\boldsymbol{\mu})\right), \end{array} $$

(21)

where $\boldsymbol {z}_{ij} = \frac {1}{2}(\boldsymbol {x}_{i}+\boldsymbol {x}_{j})$. $\boldsymbol {z}^{\prime }$ and R are defined:

$$ \begin{array}{@{}rcl@{}} \boldsymbol{z}^{\prime}_{ij} ={\boldsymbol{\varLambda}}_{g_{b}}(\boldsymbol{\varLambda}_{g_{a}}+\boldsymbol{\varLambda}_{g_{b}})^{-1}\boldsymbol{x}_{i} + \boldsymbol{\varLambda}_{g_{a}}(\boldsymbol{\varLambda}_{g_{a}}+\boldsymbol{\varLambda}_{g_{b}})^{-1}\boldsymbol{x}_{j}, \end{array} $$

(22)

$$ \begin{array}{@{}rcl@{}} \boldsymbol{R}=(\boldsymbol{\varLambda}_{g_{a}}^{-1}+\boldsymbol{\varLambda}_{g_{b}}^{-1})^{-1}+\boldsymbol{\Sigma}. \end{array} $$

(23)

1.2 A.2 Input-output covariance

According to a previous work [26], to calculate C, which is the covariance between x and z, we define input state $\boldsymbol {x}_{*}\sim \mathcal {N}(\boldsymbol {\mu }, \boldsymbol {\Sigma })$, and predict observation from $h_{g}(\boldsymbol {x}_{*}|\boldsymbol {\mu }, \boldsymbol {\Sigma })\sim \mathcal {N}(\boldsymbol {\mu }_{*}, \boldsymbol {\Sigma }_{*})$. The joint distribution is:

$$ p\left( \mathbf{x}_{*}, h_{g}\left( \mathbf{x}_{*}\right) | \boldsymbol{\mu}, \mathbf{\Sigma}\right)=\mathcal{N}\left( \left[\begin{array}{c}{\boldsymbol{\mu}} \\ {\boldsymbol{\mu}_{*}} \end{array}\right],\left[\begin{array}{cc}{\boldsymbol{\Sigma}} & \boldsymbol{C} \\ \boldsymbol{C}^{\top} & {\boldsymbol{\Sigma}_{*}} \end{array}\right]\right). $$

(24)

The covariance is represented as:

$$ \boldsymbol{C}=\mathbb{E}_{\boldsymbol{x}_{*}, h_{g}}[\boldsymbol{x}_{*}h_{g}(\boldsymbol{x}_{*})^{\top}]-\boldsymbol{\mu}\boldsymbol{\mu}_{*}^{\top}. $$

(25)

For all N samples for each dimension a = 1,...,D, we have:

$$ \begin{array}{@{}rcl@{}} \mathbb{E}_{\boldsymbol{x}_{*}, {h_{g}^{a}}}[\boldsymbol{x}_{*}{h_{g}^{a}}(\boldsymbol{x}_{*})^{\top}]&=&\int \boldsymbol{x}_{*}m_{g_{a}}(\boldsymbol{x}_{*})\mathrm{d} \boldsymbol{x}_{*}\\ &=&{\sum}_{i=1}^{N}{\upbeta}_{g_{ai}}\int \boldsymbol{x}_{*} c_{1} \boldsymbol{k}_{g_{a}}({\boldsymbol{x}}_{*})^{\top}p(\boldsymbol{x}_{*}) \boldsymbol{x}_{*}, \end{array} $$

(26)

where we define $c_{1}:=\alpha _{g_{a}}^{-2}(2\pi )^{-\frac {D}{2}}|\boldsymbol {\varLambda }_{g_{a}}|^{-\frac {1}{2}}$ so that $c_{1} \boldsymbol {k}_{g_{a}}(\boldsymbol {x}_{*})^{\top }$ becomes a normalized Gaussian distribution. The product of two Gaussian distributions, $\boldsymbol {x}_{*} c_{1} \boldsymbol {k}_{g_{a}}(\boldsymbol {x}_{*})^{\top }\times p(\boldsymbol {x}_{*})$, creates a new Gaussian $c_{2}^{-1}\mathcal {N}(\boldsymbol {x}|\boldsymbol {\phi }_{i}, \boldsymbol {\Psi })$:

$$ \begin{array}{@{}rcl@{}} c_{2}^{-1} &=& (2 \pi)^{-\frac{D}{2}}\left|\boldsymbol{\Lambda}_{g_{a}}+\mathbf{\Sigma}\right|^{-\frac{1}{2}}\\ &&\times\exp \left( -\frac{1}{2}\left( \mathbf{x}_{i}-\boldsymbol{\mu}\right)^{\top}\left( \boldsymbol{\Lambda}_{g_{a}}+\mathbf{\Sigma}\right)^{-1}\left( \mathbf{x}_{i}-\boldsymbol{\mu}\right)\right)\\ \mathbf{\Psi} &=&\left( \boldsymbol{\Lambda}_{g_{a}}^{-1}+\boldsymbol{\Sigma}^{-1}\right)^{-1} \\ \boldsymbol{\psi}_{i} &=&\mathbf{\Psi}\left( \boldsymbol{\Lambda}_{g_{a}}^{-1} \mathbf{x}_{i}+\boldsymbol{\Sigma}^{-1} \boldsymbol{\mu}\right). \end{array} $$

(27)

Considering (17) the mean of ${h_{g}^{a}}(\boldsymbol {x}_{*})$, we have:

$$ \begin{array}{@{}rcl@{}} \mathbb{E}_{\boldsymbol{x}_{*}, {h_{g}^{a}}}[\boldsymbol{x}_{*}{h_{g}^{a}}(\boldsymbol{x}_{*})]&=&{\sum}_{i=1}^{N}{\upbeta}_{g_{ai}} c_{1} c_{2}^{-1}\boldsymbol{\psi}_{i} = {\sum}_{i=1}^{N}{\upbeta}_{g_{ai}} l_{g_{ai}}\boldsymbol{\psi}_{i}\\ &=&{\sum}_{i=1}^{N}{\upbeta}_{g_{ai}} l_{g_{ai}}\mathbf{\Psi}\left( \boldsymbol{\Lambda}_{g_{a}}^{-1} \mathbf{x}_{i}+\boldsymbol{\Sigma}^{-1} \boldsymbol{\mu}\right). \end{array} $$

(28)

Since μ_∗ can be calculated following (17), thea dimension of covariance is represented as a combination of (28) with (26):

$$ \begin{array}{@{}rcl@{}} \boldsymbol{C}^{a}&=&{\sum}_{i=1}^{n} {\upbeta}_{g_{ai}} l_{g_{ai}} {}\left( \boldsymbol{\Sigma}\left( \boldsymbol{\Sigma}+\boldsymbol{\Lambda}_{g_{a}}\right)^{-1} \mathbf{x}_{i}\left( \boldsymbol{\Lambda}_{g_{a}}\left( \boldsymbol{\Sigma} +\boldsymbol{\Lambda}_{g_{a}}\right)^{-1}-\mathbf{I}\right) \boldsymbol{\mu}{}\right)\\ &=&{\sum}_{i=1}^{n} {\upbeta}_{g_{ai}} l_{g_{ai}} \boldsymbol{\Sigma}\left( \boldsymbol{\Sigma}+\boldsymbol{\Lambda}_{g_{a}}\right)^{-1}\left( \mathbf{x}_{i}-\boldsymbol{\mu}\right). \end{array} $$

(29)

1.3 A.3 Analytical moment matching of system dynamics

Consider the exact analytical expression of h_f(μ,Σ,u) with deterministic action u in (11):

$$ h_{f}(\boldsymbol{\mu}, \boldsymbol{\Sigma}, \boldsymbol{u}) \approx \int p\left( GP_{f}(\boldsymbol{x}_{*}, \boldsymbol{u})|\boldsymbol{x}_{*}, \boldsymbol{u}\right)p(\boldsymbol{x}_{*}|\boldsymbol{\mu}, \boldsymbol{\Sigma})\mathrm{d} \boldsymbol{x}_{*}. $$

(30)

Previous works [21, 22, 29] assumed that the state and action are independent by separating them in the SE kernel:

$$ \begin{array}{@{}rcl@{}} k_{f_{a}}(\boldsymbol{x}_{i}, \boldsymbol{u}_{i}, \boldsymbol{x}_{j}, \boldsymbol{u}_{j}) = k_{f_{a}}(\boldsymbol{u}_{i}, \boldsymbol{u}_{j})\times k_{f_{a}}(\boldsymbol{x}_{i}, \boldsymbol{x}_{j}). \end{array} $$

(31)

Defining $\boldsymbol {k}_{f_{a}}(\boldsymbol {u}) = k_{f_{a}}(\boldsymbol {U}, \boldsymbol {u})$, $\boldsymbol {k}_{f_{a}}(\boldsymbol {x}) = k_{f_{a}}(\boldsymbol {X}, \boldsymbol {x})$, the mean and covariance related to (4) and (5) follow:

$$ \begin{array}{@{}rcl@{}} m_{f_{a}}(\boldsymbol{x}, \boldsymbol{u}) & =& \left( \boldsymbol{k}_{f_{a}}(\boldsymbol{u})\times \boldsymbol{k}_{f_{a}}(\boldsymbol{x})\right)^{\top}\boldsymbol{\upbeta}_{f_{a}},\\ {\sigma}^{2}_{f_{a}}(\boldsymbol{x}, \boldsymbol{u}) & =& \left( k_{f_{a}}(\boldsymbol{u}, \boldsymbol{u})\times k_{f_{a}}(\boldsymbol{x}, \boldsymbol{x})\right) \\ &-& (\boldsymbol{k}_{f_{a}}(\boldsymbol{u})\times\boldsymbol{k}_{f_{a}}(\boldsymbol{x}))^{\top}(\boldsymbol{K}^{f_{a}}+\alpha^{2}_{f_{a}}\boldsymbol{I})^{-1}\\ &&\times(\boldsymbol{k}_{f_{a}}(\boldsymbol{x})\times\boldsymbol{k}_{f_{a}}(\boldsymbol{u})). \end{array} $$

(32)

Define $\boldsymbol {\upbeta }_{f_{a}}=(\boldsymbol {K}^{f_{a}}+\alpha ^{2}_{f_{a}}\boldsymbol {I})^{-1}\boldsymbol {Y}^{a}$, and the mean of h_f(μ,Σ,u) in input dimension a is calculated following previous derivations [25, 26]:

$$ \begin{array}{@{}rcl@{}} \mu_{f_{a}} &=& \int m_{f_{a}}(\boldsymbol{x}_{*}, \boldsymbol{u}) p(\boldsymbol{x}_{*}|\boldsymbol{\mu}, \boldsymbol{\Sigma}) \mathrm{d} \boldsymbol{x}_{*}\\ &=&\boldsymbol{\upbeta}_{f_{a}}^{\top}\boldsymbol{k}_{f_{a}}(\boldsymbol{u})\int \boldsymbol{k}_{f_{a}}(\boldsymbol{x}_{*}) p(\boldsymbol{x}_{*}|\boldsymbol{\mu}, \boldsymbol{\Sigma}) \boldsymbol{x}_{*} \\ &=& \boldsymbol{\upbeta}_{f_{a}}^{\top}\boldsymbol{l}_{f_{a}}. \end{array} $$

(33)

For target dimension a,b = 1,...,D and a≠b, predicted variance ${\Sigma }_{f_{aa}}$ and covariance ${\Sigma }_{f_{ab}}$ of h_f(μ,Σ,u) follow:

$$ \begin{array}{@{}rcl@{}} {\Sigma}_{f_{aa}}&=& \mathbb{E}\left[\sigma_{f_{a}}^{2}(\boldsymbol{x}, \boldsymbol{u})\right] + \mathbb{E}\left[m_{f_{a}}^{2}(\boldsymbol{x}, \boldsymbol{u})\right] - \mu_{f_{a}}^{2} \\ &=& \boldsymbol{\upbeta}_{f_{a}}^{\top}\boldsymbol{L}^{f}\boldsymbol{\upbeta}_{f_{a}} + \alpha^{2}_{f_{a}} - tr\left( (\boldsymbol{K}^{f_{a}} + \sigma_{w_{a}}^{2}\boldsymbol{I})^{-1}\boldsymbol{L}^{f}\right) - \mu_{f_{a}}^{2},\\ {\Sigma}_{f_{ab}} &=& \mathbb{E}{}\left[m_{f_{a}}(\boldsymbol{x}, \boldsymbol{u})m_{f_{b}}(\boldsymbol{x}, \boldsymbol{u})\right]{}-{} \mu_{f_{a}}\mu_{f_{b}} {}={} \boldsymbol{\upbeta}_{f_{a}}^{\top}\boldsymbol{Q}^{f}\boldsymbol{\upbeta}_{f_{b}} - \mu_{f_{a}}\mu_{f_{b}}. \end{array} $$

(34)

Define u_i,x_i as the i-th sample in U and X, and vectors $\boldsymbol {l}_{f_{a}}$ and matrices L^f, Q^f have the following elements:

$$ \begin{array}{@{}rcl@{}} l_{f_{ai}} &=& k_{f_{a}}(\boldsymbol{u}_{i}, \boldsymbol{u})\int k_{f_{a}}(\boldsymbol{x}_{i}, \boldsymbol{x}_{*}) p(\boldsymbol{x}_{*}|\boldsymbol{\mu}, \boldsymbol{\Sigma}) \mathrm{d} \boldsymbol{x}_{*}\\ &=&k_{f_{a}}(\boldsymbol{u}_{i}, \boldsymbol{u})\alpha^{2}_{f_{a}}|\boldsymbol{\Sigma}\boldsymbol{\varLambda}_{f_{a}}^{-1} + \boldsymbol{I}|^{-\frac{1}{2}} \\ &&\times\exp\left( -\frac{1}{2}(\boldsymbol{x}_{i}-\boldsymbol{\mu})^{\top}(\boldsymbol{\Sigma}+\boldsymbol{\varLambda}_{f_{a}})^{-1}(\boldsymbol{x}_{i}-\boldsymbol{\mu})\right). \end{array} $$

(35)

$$ \begin{array}{@{}rcl@{}} L_{ij}^{f} &= & k_{f_{a}}(\boldsymbol{u}_{i},\boldsymbol{u})k_{f_{a}}(\boldsymbol{u}_{j},\boldsymbol{u})\frac{k_{f_{a}}(\boldsymbol{x}_{i},\boldsymbol{\mu})k_{f_{a}}(\boldsymbol{x}_{j},\boldsymbol{\mu})}{|2\boldsymbol{\Sigma}\boldsymbol{\varLambda}_{f_{a}}^{-1} + \boldsymbol{I}|^{\frac{1}{2}}}\\ &&\times \exp\left( (\boldsymbol{z}_{ij}-\boldsymbol{\mu})^{\top}(\boldsymbol{\Sigma}+\frac{1}{2}\boldsymbol{\varLambda}_{f_{a}})^{-1}\boldsymbol{\Sigma}\boldsymbol{\varLambda}_{f_{a}}^{-1}(\boldsymbol{z}_{ij}-\boldsymbol{\mu})\right), \end{array} $$

(36)

$$ \begin{array}{@{}rcl@{}} Q_{ij}^{f} &= & \alpha_{f_{a}}^{2}\alpha_{f_{b}}^{2}k_{f_{a}}(\boldsymbol{u}_{i},\boldsymbol{u}_{j})k_{f_{b}}(\boldsymbol{u}_{i},\boldsymbol{u}_{j})|(\boldsymbol{\varLambda}_{f_{a}}^{-1}+\boldsymbol{\varLambda}_{f_{b}}^{-1})\boldsymbol{\Sigma}+\boldsymbol{I}|^{-\frac{1}{2}}\\ &&\times\exp\left( -\frac{1}{2}(\boldsymbol{x}_{i} - \boldsymbol{x}_{j})^{\top}(\boldsymbol{\varLambda}_{f_{a}} + \boldsymbol{\varLambda}_{f_{b}})^{-1}(\boldsymbol{x}_{i} - \boldsymbol{x}_{j})\right)\\ &&\times\exp\left( -\frac{1}{2}(\boldsymbol{z}_{ij}^{\prime}-\boldsymbol{\mu})^{\top}\boldsymbol{R}^{-1}(\boldsymbol{z}_{ij}^{\prime}-\boldsymbol{\mu})\right), \end{array} $$

(37)

where $\boldsymbol {z}_{ij} = \frac {1}{2}(\boldsymbol {x}_{i}+\boldsymbol {x}_{j})$, $\boldsymbol {z}^{\prime }$ and R follow (22) and (23) with subscript f instead of g.