},\mathbf{u}^{p}_{0:t-1})\mathbf{H}^{p}_t(\mathbf{x}^{p}_{t}))\mathbf{P}^{-}_{\mathbf{b}^{p}_t}(\mathbf{x}^{p}_{0:t},\mathbf{u}^{p}_{0:t-1})\end{aligned}$$ which is solvable with the initial condition $ \mathbf{P}^{+}_{\mathbf{b}^{p}_0}(\mathbf{x}^{p}_{0})=\boldsymbol{\Sigma}_{\mathbf{x}_{0}} $.

\[problem:Planning Problem\] Given an initial belief $ \mathbf{b}_{0}\in\mathbb{B} $, a goal region represented as an $ \ell_2 $-norm ball, $ \mathbf{B}_{r_g}(\mathbf{x}_g) $, of radius $ r_g $ around a goal state $ \mathbf{x}_{g}\in\mathbb{X} $, and a planning horizon of $ K>0 $, we define the following problem:

$$\begin{aligned}
\nonumber \min_{\mathbf{u}^{p}_{0:K-1}}\sum\limits_{t=1}^{K}[&\mathrm{tr}(\mathbf{W}_t \mathbf{P}^{+}_{\mathbf{b}^{p}_t}\mathbf{W}^T_t)+({\mathbf{u}}^{p}_{t-1})^{T}\mathbf{W}^{u}_{t}{\mathbf{u}}^{p}_{t-1}]
\\s.t.~~\mathbf{P}^{-}_{\mathbf{b}^{p}_t}&=\mathbf{A}^{p}_t\mathbf{P}^{+}_{\mathbf{b}^{p}_{t-1}}(\mathbf{A}^{p}_t)^T+\mathbf{G}^{p}_t\boldsymbol{\Sigma}_{\boldsymbol{\omega}_{t}}(\mathbf{G}_t)^T\label{eq:riccati 1}
\\\mathbf{S}_{\mathbf{b}^{p}_t}&=\mathbf{H}^{p}_t\mathbf{P}^{-}_{\mathbf{b}^{p}_t}(\mathbf{H}^{p}_t)^T+\mathbf{M}^{p}_t\boldsymbol{\Sigma}_{\boldsymbol{\nu}_{t}}(\mathbf{M}^{p}_t)^T\label{eq:riccati 2}
\\\mathbf{K}_{\mathbf{b}^{p}_t}&=\mathbf{P}^{-}_{\mathbf{b}^{p}_t}(\
}, \mathbf{u}^{p}_{0: t-1})\mathbf{H}^{p}_t(\mathbf{x}^{p}_{t}))\mathbf{P}^{-}_{\mathbf{b}^{p}_t}(\mathbf{x}^{p}_{0: t},\mathbf{u}^{p}_{0: t-1})\end{aligned}$$ which is solvable with the initial condition $ \mathbf{P}^{+}_{\mathbf{b}^{p}_0}(\mathbf{x}^{p}_{0})=\boldsymbol{\Sigma}_{\mathbf{x}_{0 } } $. 

 \[problem: Planning Problem\ ] Given an initial belief $ \mathbf{b}_{0}\in\mathbb{B } $, a goal area stage as an $ \ell_2 $ -norm ball, $ \mathbf{B}_{r_g}(\mathbf{x}_g) $, of radius $ r_g $ around a finish state $ \mathbf{x}_{g}\in\mathbb{X } $, and a planning horizon of $ K>0 $, we specify the following problem: 

 $ $ \begin{aligned } 
 \nonumber \min_{\mathbf{u}^{p}_{0: K-1}}\sum\limits_{t=1}^{K}[&\mathrm{tr}(\mathbf{W}_t \mathbf{P}^{+}_{\mathbf{b}^{p}_t}\mathbf{W}^T_t)+({\mathbf{u}}^{p}_{t-1})^{T}\mathbf{W}^{u}_{t}{\mathbf{u}}^{p}_{t-1 } ] 
 \\s.t.~~\mathbf{P}^{-}_{\mathbf{b}^{p}_t}&=\mathbf{A}^{p}_t\mathbf{P}^{+}_{\mathbf{b}^{p}_{t-1}}(\mathbf{A}^{p}_t)^T+\mathbf{G}^{p}_t\boldsymbol{\Sigma}_{\boldsymbol{\omega}_{t}}(\mathbf{G}_t)^T\label{eq: riccati 1 } 
 \\\mathbf{S}_{\mathbf{b}^{p}_t}&=\mathbf{H}^{p}_t\mathbf{P}^{-}_{\mathbf{b}^{p}_t}(\mathbf{H}^{p}_t)^T+\mathbf{M}^{p}_t\boldsymbol{\Sigma}_{\boldsymbol{\nu}_{t}}(\mathbf{M}^{p}_t)^T\label{eq: riccati 2 } 
 \\\mathbf{K}_{\mathbf{b}^{p}_t}&=\mathbf{P}^{-}_{\mathbf{b}^{p}_t}(\
},\matjbf{u}^{p}_{0:t-1})\mathbf{H}^{p}_t(\mathbf{x}^{p}_{t}))\oathbf{P}^{-}_{\mathbf{b}^{p}_j}(\mqthbf{x}^{'}_{0:t},\mathbr{u}^{p}_{0:t-1})\end{auigned}$$ which is solvable witi thw iniupal condition $ \mathbf{O}^{+}_{\mathbf{b}^{p}_0}(\lathbf{x}^{p}_{0})=\voldwtmbol{\Sigma}_{\mathbf{x}_{0}} $.

\[ixoblej:Ilannnnj Problem\] Given an initian belief $ \mathtf{c}_{0}\iu\mathbb{B} $, a goal region represented ws an $ \rlp_2 $-norm ball, $ \mwthbg{F}_{r_g}(\mznhnf{x}_g) $, of radius $ r_g $ around a gozl statt $ \mathbf{x}_{g}\in\mathbn{X} $, and a planning horizon of $ K>0 $, we define the vollowing ptkblqn:

$$\begin{alignea}
\nonumber \min_{\mathbf{u}^{p}_{0:K-1}}\aum\limits_{t=1}^{K}[&\mathrm{tr}(\mathbf{W}_t \matfbf{P}^{+}_{\mcthbf{b}^{p}_t}\matybd{W}^T_h)+({\kathbf{u}}^{p}_{t-1})^{T}\methbf{W}^{l}_{t}{\mathbf{u}}^{p}_{t-1}]
\\s.t.~~\mathbf{P}^{-}_{\mdthbf{b}^{p}_y}&=\mathbf{A}^{p}_t\mathnf{P}^{+}_{\mavhbf{v}^{p}_{t-1}}(\mathbf{A}^{p}_t)^T+\mathbf{G}^{p}_v\boldsymbol{\Sigma}_{\boldfymbol{\omeca}_{c}}(\mathbf{G}_t)^T\label{eq:riccqtu 1}
\\\matvbf{S}_{\kathcd{b}^{p}_g}&=\mauhbh{H}^{p}_f\mathbv{P}^{-}_{\methbf{b}^{p}_t}(\matgbf{H}^{p}_t)^T+\mathvf{M}^{p}_t\boldsymbol{\Sigms}_{\bjodsymbol{\nu}_{t}}(\mafhbf{M}^{p}_e)^T\jabel{eq:riccati 2}
\\\mathbf{K}_{\mathbf{b}^{p}_t}&=\mathbf{P}^{-}_{\mdthgf{b}^{p}_t}(\
},\mathbf{u}^{p}_{0:t-1})\mathbf{H}^{p}_t(\mathbf{x}^{p}_{t}))\mathbf{P}^{-}_{\mathbf{b}^{p}_t}(\mathbf{x}^{p}_{0:t},\mathbf{u}^{p}_{0:t-1})\end{aligned}$$ which is solvable with the initial \mathbf{P}^{+}_{\mathbf{b}^{p}_0}(\mathbf{x}^{p}_{0})=\boldsymbol{\Sigma}_{\mathbf{x}_{0}} \[problem:Planning Problem\] an initial belief region as an $ $-norm ball, $ $, of radius $ r_g $ a goal state $ \mathbf{x}_{g}\in\mathbb{X} $, and a planning horizon of $ K>0 we define the following problem: $$\begin{aligned} \nonumber \min_{\mathbf{u}^{p}_{0:K-1}}\sum\limits_{t=1}^{K}[&\mathrm{tr}(\mathbf{W}_t \mathbf{P}^{+}_{\mathbf{b}^{p}_t}\mathbf{W}^T_t)+({\mathbf{u}}^{p}_{t-1})^{T}\mathbf{W}^{u}_{t}{\mathbf{u}}^{p}_{t-1}] \\s.t.~~\mathbf{P}^{-}_{\mathbf{b}^{p}_t}&=\mathbf{A}^{p}_t\mathbf{P}^{+}_{\mathbf{b}^{p}_{t-1}}(\mathbf{A}^{p}_t)^T+\mathbf{G}^{p}_t\boldsymbol{\Sigma}_{\boldsymbol{\omega}_{t}}(\mathbf{G}_t)^T\label{eq:riccati 1} \\\mathbf{S}_{\mathbf{b}^{p}_t}&=\mathbf{H}^{p}_t\mathbf{P}^{-}_{\mathbf{b}^{p}_t}(\mathbf{H}^{p}_t)^T+\mathbf{M}^{p}_t\boldsymbol{\Sigma}_{\boldsymbol{\nu}_{t}}(\mathbf{M}^{p}_t)^T\label{eq:riccati 2}
},\mathbf{u}^{p}_{0:t-1})\mathbf{H}^{p}_t(\mathbf{x}^{P}_{t}))\mathbf{P}^{-}_{\mAthbf{B}^{p}_t}(\MatHbF{x}^{p}_{0:t},\MathBf{u}^{p}_{0:t-1})\end{aligneD}$$ WhicH is solvable with the initIal coNdITion $ \MAtHbf{P}^{+}_{\mAthbf{b}^{p}_0}(\MAtHBF{x}^{p}_{0})=\BoLdSymBoL{\siGma}_{\maThbF{x}_{0}} $.

\[problEm:Planning proBlEm\] Given an iniTIaL belief $ \matHbf{B}_{0}\in\mathbb{B} $, a gOal Region RePreSEnted As aN $ \ell_2 $-nOrm balL, $ \Mathbf{b}_{r_g}(\mathbf{X}_g) $, OF radiuS $ R_g $ arounD A GoAl stAte $ \mathbf{x}_{g}\in\mathBB{X} $, ANd a planning horIzon of $ k>0 $, wE DeFINe tHe fOllowing prObLem:

$$\beGIn{alignED}
\nONUMbeR \Min_{\mathbf{u}^{p}_{0:K-1}}\sUm\limits_{t=1}^{K}[&\mAThrM{tr}(\matHbF{W}_t \MAthbf{P}^{+}_{\MathbF{b}^{P}_T}\maThbf{W}^T_t)+({\mathBf{u}}^{p}_{T-1})^{T}\mathbf{W}^{U}_{t}{\mathBF{u}}^{p}_{t-1}]
\\s.t.~~\mAThbf{P}^{-}_{\maThbf{b}^{p}_T}&=\maThbF{A}^{p}_t\MAtHbF{P}^{+}_{\mAtHBf{b}^{P}_{T-1}}(\mAthBF{A}^{p}_T)^T+\mathbf{g}^{p}_T\bOldsyMbol{\sIGMA}_{\bolDsyMbol{\Omega}_{T}}(\mathbf{G}_t)^T\labEl{eQ:ricCAti 1}
\\\MathbF{S}_{\matHbf{b}^{P}_t}&=\MathbF{H}^{p}_t\maThbf{P}^{-}_{\MaThbf{b}^{p}_t}(\mathbf{H}^{p}_T)^T+\maThbf{M}^{p}_t\boLdsYmBol{\siGma}_{\boLDsymboL{\nu}_{T}}(\maThbf{M}^{p}_t)^t\label{eQ:RicCaTI 2}
\\\MAtHbf{K}_{\mathbf{b}^{p}_t}&=\mathbF{P}^{-}_{\MAThBf{b}^{p}_t}(\
},\mathbf{u}^{p}_{0:t-1})\ mathbf{H}^ {p}_t (\m ath bf {x}^ {p}_ {t}))\mathbf{P } ^{-} _{\mathbf{b}^{p}_t}(\m athbf {x } ^{p} _ {0 :t},\ mathbf{ u }^ { p }_{ 0: t- 1}) \e n d{ align ed} $$ whic h is solva ble  w ith the init i al  condition  $\mathbf{P}^{ +}_ {\math bf {b} ^ {p}_0 }(\ mathb f{x}^{ p }_{0}) =\boldsym bo l {\Sigm a }_{\mat h b f{ x}_{ 0}} $.

\[problem : Pl a nning Problem\ ] Give na ni n iti albelief $ \ ma thbf{ b }_{0}\i n \m a t h bb{ B } $, a goal r egion repre s ent ed asan  $\ ell_2$-nor mb all , $ \mathbf {B}_ {r_g}(\ma thbf{x } _g) $,o f radiu s $ r_ g $  ar ound ago alst a te$  \ mat h bf{ x}_{g}\i n\ ma thbb{ X} $ , a n d apla nnin g hor izon of $ K>0  $,  wed efi ne th e fol lowi ng  prob lem:
$$\be gi n{aligned}
\non umbe r \min_{\ mat hb f{u }^ {p}_{ 0 :K-1}} \su m\l imits_{ t=1}^{K } [&\ ma t h r m{ tr}(\mathbf{W}_t \ ma t h bf {P}^{+}_ {\math b f{ b} ^ {p}_t}\m at hbf {W}^ T _ t)+({ \mat h bf {u}}^{p} _{t-1} ) ^{ T} \mathbf {W }^{u}_ {t }{\ mat hbf{u } }^{p }_{t-1 }]
\\s.t .~~\m a thbf{P}^{-}_{\ m athbf{b}^{p}_ t }& = \ ma t hbf{ A}^ {p}_t\mathb f{P} ^ {+}_ {\ma t hb f{b } ^{p}_ {t-1} }( \ ma t hbf{A}^{p}_t)^T+\ma th bf{G}^ {p}_t \boldsymbol{\ Sigma}_{\b o l d symbol{\ omeg a }_ { t}}(\mathbf{G} _t)^T \label{eq: r iccati 1 }
\\\ mathbf{S }_{\mathb f { b}^{p}_t }&= \ma thb f{H } ^ {p }_t\mathbf{P} ^ { -}_{ \m athbf{b }^{ p}_t}(\ mat hbf {H} ^{p }_ t)^T+\mat hbf{M}^{ p} _t \b ol dsy mbol{ \ Sigma}_{ \b old sy mbo l{\nu } _{t}}( \math bf{M }^ {p } _t) ^T\labe l {e q : ricc at i2}
\ \\m at hbf{K }_{\ m ath bf{b}^{ p}_t}&=\m ath b f{P} ^{ -} _{\math bf{b}^{p}_t}( \
},\mathbf{u}^{p}_{0:t-1})\mathbf{H}^{p}_t(\mathbf{x}^{p}_{t}))\mathbf{P}^{-}_{\mathbf{b}^{p}_t}(\mathbf{x}^{p}_{0:t},\mathbf{u}^{p}_{0:t-1})\end{aligned}$$ which_is solvable_with the initial condition_$ \mathbf{P}^{+}_{\mathbf{b}^{p}_0}(\mathbf{x}^{p}_{0})=\boldsymbol{\Sigma}_{\mathbf{x}_{0}}_$.

\[problem:Planning_Problem\] Given_an_initial belief $_\mathbf{b}_{0}\in\mathbb{B} $, a_goal region represented as_an $ \ell_2_$-norm_ball, $ \mathbf{B}_{r_g}(\mathbf{x}_g) $, of radius $ r_g $ around a goal state $_\mathbf{x}_{g}\in\mathbb{X}_$, and_a_planning_horizon of $ K>0 $,_we define the following problem:

$$\begin{aligned}
\nonumber_\min_{\mathbf{u}^{p}_{0:K-1}}\sum\limits_{t=1}^{K}[&\mathrm{tr}(\mathbf{W}_t \mathbf{P}^{+}_{\mathbf{b}^{p}_t}\mathbf{W}^T_t)+({\mathbf{u}}^{p}_{t-1})^{T}\mathbf{W}^{u}_{t}{\mathbf{u}}^{p}_{t-1}]
\\s.t.~~\mathbf{P}^{-}_{\mathbf{b}^{p}_t}&=\mathbf{A}^{p}_t\mathbf{P}^{+}_{\mathbf{b}^{p}_{t-1}}(\mathbf{A}^{p}_t)^T+\mathbf{G}^{p}_t\boldsymbol{\Sigma}_{\boldsymbol{\omega}_{t}}(\mathbf{G}_t)^T\label{eq:riccati_1}
\\\mathbf{S}_{\mathbf{b}^{p}_t}&=\mathbf{H}^{p}_t\mathbf{P}^{-}_{\mathbf{b}^{p}_t}(\mathbf{H}^{p}_t)^T+\mathbf{M}^{p}_t\boldsymbol{\Sigma}_{\boldsymbol{\nu}_{t}}(\mathbf{M}^{p}_t)^T\label{eq:riccati 2}
\\\mathbf{K}_{\mathbf{b}^{p}_t}&=\mathbf{P}^{-}_{\mathbf{b}^{p}_t}(\