CSCE 452/752 Robotics and Spatial Intelligence, Fall 2025

Homework 10 Solutions

A robot moves through a one-dimensional state space $X=\mathbb{R}$ by choosing actions from a one-dimensional action space $U=\mathbb{R}$. The state transition function is \[ x_{k+1} = x_k + 2u_k + 3\theta_k, \] in which the noise value $\theta_k$ is selected randomly from a Gaussian distribution with mean $0$ and variance $0.5$. At each time step, the robot receives an observation from a one-dimensional observation space $Y=\mathbb{R}$, according to the observation function \[ y_k = x_k + 4\psi_k,\] in which the noise value $\psi_k$ is selected randomly from a Gaussian distribution with mean $0$ and variance $1.5$.
At a certain time step $k$, the robot knows that its most likely state is $\mu_k = 5$, with variance $\Sigma_k=0.3$. The robot receives an observation $y_k = 7$ and executes an action $u_k=0.5$.
Using the Kalman filter, compute the most likely value $\mu_{k+1}$ of $x_{k+1}$ and the variance $\Sigma_{k+1}$ of that estimate. Show your work.
Solution:
We are given: \begin{eqnarray*} A &=& 1 \\ B &=& 2 \\ G &=& 3 \\ C &=& 1 \\ H &=& 4 \\ \mu_k &=& 5 \\ \Sigma_k &=& 0.3 \\ \Sigma_\theta &=& 0.5 \\ \Sigma_\psi &=& 1.5 \\ u_k &=& 0.5 \\ y_k &=& 7 \end{eqnarray*} From there, apply the steps of the filter:
  • Compute $\Sigma^\prime_{k+1}$: \begin{eqnarray*} \Sigma^\prime_{k+1} &=& A \Sigma_k A^\top + G \Sigma_\theta G^\top \\ &=& 1 \cdot 0.3 \cdot 1 + 3 \cdot 0.5 \cdot 3 \\ &=& 0.3 + 4.5 = 4.8 \end{eqnarray*}
  • Compute $L_{k+1}$: \begin{eqnarray*} L_{k+1} &=& \Sigma^\prime_{k+1}C^\top ( C \Sigma^\prime_{k+1} C^\top + H \Sigma_\psi H )^{-1} \\ &=& 4.8 \cdot 1 \cdot ( 1 \cdot 4.8 \cdot 1 + 4 \cdot 1.5 \cdot 4 )^{-1} \\ &=& 4.8 \cdot (28.8)^{-1} = 1/6 \end{eqnarray*}
  • Compute $\mu_{k+1}$: \begin{eqnarray*} \mu_{k+1} &=& A \mu_k + B u_k + L_{k+1}( y_k - C(A \mu_k + B u_k) ) \\ &=& 1 \cdot 5 + 2 \cdot 0.5 + (1/6)( 7 - 1 \cdot (1 \cdot 5 + 2 \cdot 0.5)) \\ &=& 6 + (1/6) \\ &=& 37/6 \end{eqnarray*}
  • Compute $\Sigma_{k+1}$: \begin{eqnarray*} \Sigma_{k+1} &=& (I - L_{k+1}C)\Sigma^\prime_{k+1} \\ &=& (1 - (1/6)\cdot 1)\cdot 4.8 \\ &=& 4 \end{eqnarray*}