# CS 530 - Lecture 06

## State Space Models

Bernhard Firner

2026-02-12

---

# Reading

* See section 14.4.2 through 14.4.4 in Russel and Norvig

---

## Limitations of Discrete Models

* HMMs become intractable when solving discrete problems
* Not immediately
  * HMMs with Gaussian states are used in speed processing
  * But many real world problems have too many states

---

## Continuous Examples

* A common filtering problem is tracking
  * Track a ball in the air
  * Track a vehicle on the road using successive bounding box estimates
  * Track a landing pad on a ship when attempting to land a drone

---

## Tracking Meaningless Features

* We may even track "non-objects"
  * For example, if you want to track your robot's movement, you can track the world around you
* Is everything moving left? Then the robot is spinning right
  * Is everything in the middle slowly moving and everything near the edge moving quickly?
  * Then the robot is moving forwards

---

## Feature Tracking

* The ability to track arbitrary features is useful
   * A key component of something called "Simultaneous Localization and Mapping"
* A combination of statistics and geometry
  * Used in the trail navigating drone we discussed previously

---

## Smoothing

* Similar to the forward-backward algorithm
* Fixed lag smoothing of wind speed measurements, for example
* Or offline, cleaning up datasets

---

## Example

</div>
<div class="col">

</div>
</div>

---

## Continuous Models

* Everything we learned from Hidden Markov Models applies to continuous spaces
* But now the hidden states are continuous rather than discrete
* We may also have to worry about the nature of our time dimension
  * But let's not overcomplicate things right away

---

## Notation

* Our hidden state at time $t$ will be $z_t$
* Our observation at time $t$ will be $y_t$
* What influences state transitions?
  * We add noise in the system, $\epsilon$, to the system model
  * We also assume that there could be inputs or control signals, which we'll call $u$

---

## Linear Gaussian Transitions

* We will usually assume that things are linear and gaussian
* State transitions are then a linear equation:
  * $z_t = A_tz_{t-1} + B_tu_t + \epsilon_t$
  * $u$ represents a control signal and may not always be present

---

## Tracking Example

* Tracking is a common example of continuous space problem
* The hidden state is made up of the position and velocity of the tracked object
  * e.g. $z_t = [x_t, y_t, z_t, v_{x,t}, v_{y,t}, v_{z,t}]$
* In an image we may only have two dimensions, or we could add acceleration, etc
  * The point is that the hidden state is composed of continuous values

---

## State Transitions

* Let's say that the object being tracked is moving at constant velocity
  * We get new samples at some time interval, $\Delta$
  * $z_t = A_tz_{t-1}$

<p><math display="block" xmlns="http://www.w3.org/1998/Math/MathML"><semantics><mrow><msub><mi>z</mi><mi>t</mi></msub><mo>=</mo><mrow><mo stretchy="true" form="prefix">(</mo><mtable><mtr><mtd columnalign="center" style="text-align: center"><mn>1</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mi>Δ</mi></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>1</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mi>Δ</mi></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>1</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>1</mn></mtd></mtr></mtable><mo stretchy="true" form="postfix">)</mo></mrow><mrow><mo stretchy="true" form="prefix">(</mo><mtable><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>x</mi><mrow><mi>t</mi><mo>−</mo><mn>1</mn></mrow></msub></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>y</mi><mrow><mi>t</mi><mo>−</mo><mn>1</mn></mrow></msub></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>v</mi><mrow><mi>x</mi><mo>,</mo><mi>t</mi><mo>−</mo><mn>1</mn></mrow></msub></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>v</mi><mrow><mi>y</mi><mo>,</mo><mi>t</mi><mo>−</mo><mn>1</mn></mrow></msub></mtd></mtr></mtable><mo stretchy="true" form="postfix">)</mo></mrow></mrow><annotation encoding="application/x-tex">
z_t= 
\begin{pmatrix}
 1 &   0  & \Delta   & 0 \\
 0 &   1   & 0  & \Delta \\
 0 &   0   & 1   & 0 \\
 0 &   0   & 0   & 1
\end{pmatrix}
\begin{pmatrix}
 x_{t-1} \\
 y_{t-1} \\
 v_{x,t-1} \\
 v_{y,t-1} 
\end{pmatrix}</annotation></semantics></math></p>

---

## Noise

* Any real-world, continuous system has noise
  * Maybe there is wind slightly changing the object's position
  * $z_t = A_tz_{t-1} + \epsilon_t$

<p><math display="block" xmlns="http://www.w3.org/1998/Math/MathML"><semantics><mrow><msub><mi>z</mi><mi>t</mi></msub><mo>=</mo><mrow><mo stretchy="true" form="prefix">(</mo><mtable><mtr><mtd columnalign="center" style="text-align: center"><mn>1</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mi>Δ</mi></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>1</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mi>Δ</mi></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>1</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>1</mn></mtd></mtr></mtable><mo stretchy="true" form="postfix">)</mo></mrow><mrow><mo stretchy="true" form="prefix">(</mo><mtable><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>x</mi><mrow><mi>t</mi><mo>−</mo><mn>1</mn></mrow></msub></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>y</mi><mrow><mi>t</mi><mo>−</mo><mn>1</mn></mrow></msub></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>v</mi><mrow><mi>x</mi><mo>,</mo><mi>t</mi><mo>−</mo><mn>1</mn></mrow></msub></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>v</mi><mrow><mi>y</mi><mo>,</mo><mi>t</mi><mo>−</mo><mn>1</mn></mrow></msub></mtd></mtr></mtable><mo stretchy="true" form="postfix">)</mo></mrow><mo>+</mo><mrow><mo stretchy="true" form="prefix">(</mo><mtable><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>ϵ</mi><mrow><mi>x</mi><mo>,</mo><mi>t</mi></mrow></msub></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>ϵ</mi><mrow><mi>y</mi><mo>,</mo><mi>t</mi></mrow></msub></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>ϵ</mi><mrow><mi>v</mi><mi>x</mi><mo>,</mo><mi>t</mi></mrow></msub></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>ϵ</mi><mrow><mi>v</mi><mi>y</mi><mo>,</mo><mi>t</mi></mrow></msub></mtd></mtr></mtable><mo stretchy="true" form="postfix">)</mo></mrow></mrow><annotation encoding="application/x-tex">
z_t= 
\begin{pmatrix}
 1 &   0  & \Delta   & 0 \\
 0 &   1   & 0  & \Delta \\
 0 &   0   & 1   & 0 \\
 0 &   0   & 0   & 1
\end{pmatrix}
\begin{pmatrix}
 x_{t-1} \\
 y_{t-1} \\
 v_{x,t-1} \\
 v_{y,t-1} 
\end{pmatrix}
+
\begin{pmatrix}
\epsilon_{x,t} \\
\epsilon_{y,t} \\
\epsilon_{vx,t} \\
\epsilon_{vy,t} 
\end{pmatrix} </annotation></semantics></math></p>

---

## Linearity

* Notice that these are linear equations
  * As long as our transformation is linear, we can filter it
* This also allows us to combine multiple data sources
  * Something called sensor fusion

---

## Hidden States

* Those equations have us observing the entire state
  * But what if it is not directly observed?
* For example, let's say we are tracking an object in an image
  * The $x$ and $y$ pixel locations are observable
  * But the velocity is not

---

## Observations

* We have an observation, $y_t$
  * It comes from the hidden state, with some added observational noise
  * We'll call the noise here $\delta$ to distinguish it from system noise, $\epsilon$
* $y_t = C_tz_t + \delta_t$

---

## Observation Example

* So if we are observing locations from a state that contains location and velocity:

<p><math display="block" xmlns="http://www.w3.org/1998/Math/MathML"><semantics><mrow><msub><mi>y</mi><mi>t</mi></msub><mo>=</mo><mrow><mo stretchy="true" form="prefix">(</mo><mtable><mtr><mtd columnalign="center" style="text-align: center"><mn>1</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>1</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd><mtd columnalign="center" style="text-align: center"><mn>0</mn></mtd></mtr></mtable><mo stretchy="true" form="postfix">)</mo></mrow><mrow><mo stretchy="true" form="prefix">(</mo><mtable><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>x</mi><mrow><mi>t</mi><mo>−</mo><mn>1</mn></mrow></msub></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>y</mi><mrow><mi>t</mi><mo>−</mo><mn>1</mn></mrow></msub></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>v</mi><mrow><mi>x</mi><mo>,</mo><mi>t</mi><mo>−</mo><mn>1</mn></mrow></msub></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>v</mi><mrow><mi>y</mi><mo>,</mo><mi>t</mi><mo>−</mo><mn>1</mn></mrow></msub></mtd></mtr></mtable><mo stretchy="true" form="postfix">)</mo></mrow><mo>+</mo><mrow><mo stretchy="true" form="prefix">(</mo><mtable><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>δ</mi><mrow><mi>x</mi><mo>,</mo><mi>t</mi></mrow></msub></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>δ</mi><mrow><mi>y</mi><mo>,</mo><mi>t</mi></mrow></msub></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>δ</mi><mrow><mi>v</mi><mi>x</mi><mo>,</mo><mi>t</mi></mrow></msub></mtd></mtr><mtr><mtd columnalign="center" style="text-align: center"><msub><mi>δ</mi><mrow><mi>v</mi><mi>y</mi><mo>,</mo><mi>t</mi></mrow></msub></mtd></mtr></mtable><mo stretchy="true" form="postfix">)</mo></mrow></mrow><annotation encoding="application/x-tex">
y_t= 
\begin{pmatrix}
 1 &   0  & 0   & 0 \\
 0 &   1   & 0  & 0
\end{pmatrix}
\begin{pmatrix}
 x_{t-1} \\
 y_{t-1} \\
 v_{x,t-1} \\
 v_{y,t-1} 
\end{pmatrix}
+
\begin{pmatrix}
\delta_{x,t} \\
\delta_{y,t} \\
\delta_{vx,t} \\
\delta_{vy,t} 
\end{pmatrix} </annotation></semantics></math></p>

---

## Kalman Filtering

* There are multiple algorithms for making predictions in the continuous space
  * But the Kalman algorithms are the most reflexively used
* The online case is similar to the forward application in Markov chains
* The main assumption of Kalman filters is that all noise is linear and Gaussian
  * $p(z_t | y_{1:t}, u_{t:t}) = \mathcal{N}(z_t|\mu_t,\Sigma_t)$

---

## Prediction and Measurement

* Prediction comes from the mean and covariance matrix estimates, along with the transition matrix
* This is similar to the categorical predictions of a HMM coming from the previous step and the transition matrix
* $p(z_t|y_{1:t-1,u_{1:t}}) = \mathcal{N}(z_t|\mu_{t|t-1},\Sigma_{t|t-1})$

---

## Mean and Covariance

* $p(z_t|y_{1:t-1,u_{1:t}}) = \mathcal{N}(z_t|\mu_{t|t-1},\Sigma_{t|t-1})$
* $\mu_{t|t-1} \triangleq A_t\mu_{t-1} + B_tu_t$
* $\mu_{t|t-1} \triangleq A_t\mu_{t-1}A_t^T + Q_t$

---

## Measurement

* Once we observe $y_t$, we measure and update
* $p(z_t|y_t,y_{1:t-1,u_{1:t}}) \propto p(y_t|z_t,u_t)p(z_t|y_{1:t},u_{1:t})$
* With gaussians, the probability of the hidden state is:
  * $p(z_t|y_{1:t},u_t) = \mathrm{N}(z_t|\mu_t,\Sigma_t)$
* Now we need to update $\mu$ and $\Sigma$

---

## Measurement and Mean Update

* We update the mean from the previous mean and a correction factor times the error signal
  * We'll call the correction the Kalman gain: $K_t$
  * And the error is the residual, or innovation: $r_t$
     * $r_t \triangleq y_t - \hat{y}_t$
  * $\mu_t = \mu_{t|t-1} + K_tr_t$

---

## Measurement and Covariance Update

* Recall that $C_t$ converts from the hidden state to the observation
  * $\Sigma_t = (I - K_tC_t)\Sigma_{t|t-1}$

---

## Kalman Gain Matrix

* $K_t \triangleq \Sigma_{t|t-1}C_t^TS_t^-1$
* Notice that we are inverting a matrix here, which takes $O(|y_t|^3)$
  * There are other methods of calculation
* If we trust our data (the prior is strong) or our sensors are very noisy, $|K_t|$ will be small
* If the sensors are precise and have low variance, $|k_t|$ is large, and we heavily weight corrections

-v-

* Code is something like this

```python
class KalmanFilter:
    def __init__(self, dt, system_noise, measurement_noise):
        # State vector is x,y,z acceleration, velocity, and position
        self.x = numpy.zeros((9, 1))

# Transition matrix
        self.A = numpy.array([[1, 0, 0, 0, 0, 0, 0, 0, 0], # Acceleration comes from itself
                              [0, 1, 0, 0, 0, 0, 0, 0, 0],
                              [0, 0, 1, 0, 0, 0, 0, 0, 0],
                              [dt, 0, 0, 1, 0, 0, 0, 0, 0], # Velocity is the current estimate plus the time delta * accel
                              [0, dt, 0, 0, 1, 0, 0, 0, 0],
                              [0, 0, dt, 0, 0, 1, 0, 0, 0],
                              [0, 0, 0, dt, 0, 0, 1, 0, 0], # x, y, z location are the current estimate + dt * velocity
                              [0, 0, 0, 0, dt, 0, 0, 1, 0],
                              [0, 0, 0, 0, 0, dt, 0, 0, 1]])

# Our observation matrix is only the acceleration
        self.C = numpy.array([[1, 0, 0, 0, 0, 0, 0, 0, 0],
                              [0, 1, 0, 0, 0, 0, 0, 0, 0],
                              [0, 0, 1, 0, 0, 0, 0, 0, 0]])

# Initial uncertainty (Sigma, the covariance matrix)
        self.P = numpy.eye(9) * 1000
        # System noise (sigma in the equations)
        self.Q = numpy.eye(9)
        # Measurement noise (delta in the equations)
        self.R = numpy.eye(3)

def predict(self):
        # Update using the transition matrix
        self.x = numpy.dot(self.A, self.x)
        self.P = numpy.dot(numpy.dot(self.A, self.P), self.A.T) + self.Q

def update(self, z):
        z = z.reshape(-1, 1)
        # Update the state
        y = z - numpy.dot(self.C, numpy.dot(self.P, self.C.T)) + self.R
        # Update the variance
        S = numpy.dot(self.C, numpy.dot(self.P, self.C.T)) + self.R
        # Kalman gain matrix
        K = numpy.dot(numpy.dot(self.P, self.C.T), numpy.linalg.inv(S))

# Update from current estimate and the new observation
        self.x = self.x + numpy.dot(K, y)
        self.P = numpy.dot((numpy.eye(len(self.P)) - numpy.dot(K, self.C)), self.P)
```

---

## HMM Equivalent

* This gives us smoothing
* What about the localization-type problem?
  * Can we solve for a maximum likelihood?
* It turns out that gaussians are bad for this

---

## Non Gaussian Events

* Many real-world scenarios are restricted by the environment
  * e.g. a bird must go around a tree, not through it
* But if it is just as likely to go left and right, the most "likely" location is the middle of the tree

---

## Particle Filters

* The solution to this problem is a particle filter
  * Which is just a heuristic algorithm
* It is very similar to our HMM solution
  * But is just an algorithm

---

## Particle Filter

* This deserves an animation, so I'll try to draw it