Skip to content

Commit

Permalink
kalman example
Browse files Browse the repository at this point in the history
  • Loading branch information
@iBoot32
iBoot32 committed Jan 2, 2024
1 parent db84b4d commit 74f2fd9
Showing 1 changed file with 132 additions and 5 deletions.
137 changes: 132 additions & 5 deletions docs/algorithm/operations/documentation/kalman.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,14 +46,15 @@ $$
$$

This gives rise to the **state transition matrix** $A$, which describes how the state of the system changes over time. In our case, this is the kinematic model we apply to the state vector:

$$
A = \begin{bmatrix} 1 & 0 & 0 & \triangle t & 0 & 0 \\ 0 & 1 & 0 & 0 & \triangle t & 0 \\ 0 & 0 & 1 & 0 & 0 & \triangle t \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}
$$

We'll then define the covariance matrix $P$, which describes the covariance (or uncertainty) correlation between state variables. The $[i, j]$-th entry corresponds to the correlation between uncertainty of state variables $i$ and $j$. We'll initialize this to a diagonal matrix of ones, since we don't know anything about the covariance of the state vector in the beginning:
We'll then define the covariance matrix $P$, which describes the covariance (or uncertainty) correlation between state variables. The $[i, j]$-th entry corresponds to the correlation between uncertainty of state variables $i$ and $j$. We'll initialize this to a diagonal matrix of 10000 as an initial guess. This indicates that we're very uncertain about the state of the system in the beginning.

$$
P = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}
P = \begin{bmatrix} 10000 & 0 & 0 & 0 & 0 & 0 \\ 0 & 10000 & 0 & 0 & 0 & 0 \\ 0 & 0 & 10000 & 0 & 0 & 0 \\ 0 & 0 & 0 & 10000 & 0 & 0 \\ 0 & 0 & 0 & 0 & 10000 & 0 \\ 0 & 0 & 0 & 0 & 0 & 10000 \end{bmatrix}
$$

We also need to represent the uncertainty of our measurement (the pose of the target robot as estimated by solvePnP) and the uncertainty of our prediction (the pose of the target robot as estimated by our kinematic model). We can do this by defining the **measurement covariance matrix** $R$ and the **process noise covariance matrix** $Q$. $Q$ will be initialized to a diagonal matrix of 0.1 as an initial guess. $R$ will be initialized to a diagonal matrix of 5 as an initial guess. This indicates that we're more confident in our prediction via kinematics than our measurement.
Expand All @@ -79,14 +80,15 @@ $$
More rigorously, we should be calling this $Z_t$, which is the measurement at time $t$.

Now we'll form our pose prediction based on the kinematic model we defined earlier (this should be familiar from above):

$$
\hat{X} = A * X = \begin{bmatrix} 1 & 0 & 0 & \triangle t & 0 & 0 \\ 0 & 1 & 0 & 0 & \triangle t & 0 \\ 0 & 0 & 1 & 0 & 0 & \triangle t \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} * \begin{bmatrix} x_0 \\ y_0 \\ z_0 \\ v_{x_0} \\ v_{y_0} \\ v_{z_0} \end{bmatrix} = \begin{bmatrix} x_0 + v_{x_0} * \triangle t \\ y_0 + v_{y_0} * \triangle t \\ z_0 + v_{z_0} * \triangle t \\ v_{x_0} \\ v_{y_0} \\ v_{z_0} \end{bmatrix}
$$

The covariance of the predicted state vector is given by $P_t$:
The covariance of the predicted state vector is given by $\hat{P}_t$:

$$
P_t = APA^T + Q = \begin{bmatrix} 1 & 0 & 0 & dt & 0 & 0 \\ 0 & 1 & 0 & 0 & dt & 0 \\ 0 & 0 & 1 & 0 & 0 & dt \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & dt & 0 & 0 \\ 0 & 1 & 0 & 0 & dt & 0 \\ 0 & 0 & 1 & 0 & 0 & dt \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}^T + \begin{bmatrix} 0.1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0.1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0.1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0.1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0.1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0.1 \end{bmatrix}
\hat{P}_t = APA^T + Q = \begin{bmatrix} 1 & 0 & 0 & dt & 0 & 0 \\ 0 & 1 & 0 & 0 & dt & 0 \\ 0 & 0 & 1 & 0 & 0 & dt \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & dt & 0 & 0 \\ 0 & 1 & 0 & 0 & dt & 0 \\ 0 & 0 & 1 & 0 & 0 & dt \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}^T + \begin{bmatrix} 0.1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0.1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0.1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0.1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0.1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0.1 \end{bmatrix}
$$

We can then define the **innovation of measurement** $y_t$ as the difference between our measurement and our prediction:
Expand Down Expand Up @@ -134,4 +136,129 @@ dst[2] = this->X(2, 0); // predicted z
dst[3] = this->X(3, 0); // predicted vx
dst[4] = this->X(4, 0); // predicted vy
dst[5] = this->X(5, 0); // predicted vz
```
```
# Running an Example
Start by assuming we've initialized and reset the Kalman filter. We'll also assume that we have a pose estimate from solvePnP after 0.1 seconds (dt):
Measurement = $\begin {bmatrix} x \\ y \\ z \end{bmatrix} = \begin{bmatrix} 10 \\ 20 \\ 40 \end{bmatrix}$ in millimeters
```cpp
this->X_t_measure(0, 0) = pos_x;
this->X_t_measure(1, 0) = pos_y;
this->X_t_measure(2, 0) = pos_z;
```

$$
X_{measured_{t}} = z_t = \begin{bmatrix} 10 \\ 20 \\ 40 \end{bmatrix}
$$

Putting `dt` into our state transition matrix $A$:

```cpp
this->A(0, 3) = dt;
this->A(1, 4) = dt;
this->A(2, 5) = dt;
```
$$
A = \begin{bmatrix} 1 & 0 & 0 & \triangle t & 0 & 0 \\ 0 & 1 & 0 & 0 & \triangle t & 0 \\ 0 & 0 & 1 & 0 & 0 & \triangle t \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & 0.1 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0.1 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0.1 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}
$$
Updating our predicted state vector $\hat{X}$:
```cpp
this->X_hat_t = this->A * this->X;
```
$$
\hat{X} = A * X = \begin{bmatrix} 1 & 0 & 0 & 0.1 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0.1 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0.1 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \end{bmatrix}
$$

```cpp
this->P_hat_t = this->A * this->P * this->A.transpose() + this->Q;
```
$$
\hat{P}_t = APA^T + Q = \begin{bmatrix} 1 & 0 & 0 & 0.1 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0.1 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0.1 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 10000 & 0 & 0 & 0 & 0 & 0 \\ 0 & 10000 & 0 & 0 & 0 & 0 \\ 0 & 0 & 10000 & 0 & 0 & 0 \\ 0 & 0 & 0 & 10000 & 0 & 0 \\ 0 & 0 & 0 & 0 & 10000 & 0 \\ 0 & 0 & 0 & 0 & 0 & 10000 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & 0.1 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0.1 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0.1 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}^T + \begin{bmatrix} 0.1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0.1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0.1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0.1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0.1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0.1 \end{bmatrix} \newline
= \begin{bmatrix} 10100.1 & 0 & 0 & 1000 & 0 & 0 \\ 0 & 10100.1 & 0 & 0 & 1000 & 0 \\ 0 & 0 & 10100.1 & 0 & 0 & 1000 \\ 1000 & 0 & 0 & 10000.1 & 0 & 0 \\ 0 & 1000 & 0 & 0 & 10000.1 & 0 \\ 0 & 0 & 1000 & 0 & 0 & 10000.1 \end{bmatrix}
$$

```cpp
this->z_t = Eigen::Matrix<float, 3, 3>::Identity() * this->X_t_measure;
```
$$
X_{measured_{t}} = z_t = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 10 \\ 20 \\ 40 \end{bmatrix} = \begin{bmatrix} 10 \\ 20 \\ 40 \end{bmatrix}
$$

```cpp
this->y_t = this->z_t - this->H * this->X_hat_t;
```

$$
y_t = z_t - H\hat{X_t} \newline \space \newline
y_t = \begin{bmatrix} 10 \\ 20 \\ 40 \end{bmatrix} - \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \end{bmatrix} * \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \end{bmatrix} = \begin{bmatrix} 10 \\ 20 \\ 40 \end{bmatrix}
$$

```cpp
this->S_t = this->H * this->P_hat_t * this->H.transpose() + this->R;
```

$$
S_t = H\hat{P_t}H^T + R = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} 10100.1 & 0 & 0 & 1000 & 0 & 0 \\ 0 & 10100.1 & 0 & 0 & 1000 & 0 \\ 0 & 0 & 10100.1 & 0 & 0 & 1000 \\ 1000 & 0 & 0 & 10000.1 & 0 & 0 \\ 0 & 1000 & 0 & 0 & 10000.1 & 0 \\ 0 & 0 & 1000 & 0 & 0 & 10000.1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix} + \begin{bmatrix} 5 & 0 & 0 \\ 0 & 5 & 0 \\ 0 & 0 & 5 \end{bmatrix} \newline
= \begin{bmatrix} 10105.1 & 0 & 0 \\ 0 & 10105.1 & 0 \\ 0 & 0 & 10105.1 \end{bmatrix}
$$

```cpp
this->K = this->P_hat_t * this->H.transpose() * this->s_t.inverse();
```

$$
K = \hat{P_t}H^TS_t^{-1} = \begin{bmatrix} 10100.1 & 0 & 0 & 1000 & 0 & 0 \\ 0 & 10100.1 & 0 & 0 & 1000 & 0 \\ 0 & 0 & 10100.1 & 0 & 0 & 1000 \\ 1000 & 0 & 0 & 10000.1 & 0 & 0 \\ 0 & 1000 & 0 & 0 & 10000.1 & 0 \\ 0 & 0 & 1000 & 0 & 0 & 10000.1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} 10105.1 & 0 & 0 \\ 0 & 10105.1 & 0 \\ 0 & 0 & 10105.1 \end{bmatrix}^{-1} \newline
= \begin{bmatrix} 0.9999 & 0 & 0 \\ 0 & 0.9999 & 0 \\ 0 & 0 & 0.9999 \\ 0.0999 & 0 & 0 \\ 0 & 0.0999 & 0 \\ 0 & 0 & 0.0999 \end{bmatrix}
$$

```cpp
this->P = (Eigen::Matrix<float, 6, 6>::Identity() - this->K * this->H) * this->P_hat_t;
```

$$
P_t = (I - KH)\hat{P_t} = (I - K \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \end{bmatrix}) \hat{P_t} \newline
= \left( \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix} - \begin{bmatrix} 0.9999 & 0 & 0 \\ 0 & 0.9999 & 0 \\ 0 & 0 & 0.9999 \\ 0.0999 & 0 & 0 \\ 0 & 0.0999 & 0 \\ 0 & 0 & 0.0999 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \end{bmatrix}\right) \begin{bmatrix} 10100.1 & 0 & 0 & 1000 & 0 & 0 \\ 0 & 10100.1 & 0 & 0 & 1000 & 0 \\ 0 & 0 & 10100.1 & 0 & 0 & 1000 \\ 1000 & 0 & 0 & 10000.1 & 0 & 0 \\ 0 & 1000 & 0 & 0 & 10000.1 & 0 \\ 0 & 0 & 1000 & 0 & 0 & 10000.1 \end{bmatrix} \newline
= \begin{bmatrix} 4.9967 & 0 & 0 & 0.4947 & 0 & 0 \\ 0 & 4.9967 & 0 & 0 & 0.4947 & 0 \\ 0 & 0 & 4.9967 & 0 & 0 & 0.4947 \\ 0.4947 & 0 & 0 & 9901.14 & 0 & 0 \\ 0 & 0.4947 & 0 & 0 & 9901.14 & 0 \\ 0 & 0 & 0.4947 & 0 & 0 & 9901.14 \end{bmatrix}
$$

```cpp
this->X = this->X_hat_t + this->K * this->y_t;
```

$$
\begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \\ 0 \\ 0 \end{bmatrix} + \begin{bmatrix} 0.9999 & 0 & 0 \\ 0 & 0.9999 & 0 \\ 0 & 0 & 0.9999 \\ 0.0999 & 0 & 0 \\ 0 & 0.0999 & 0 \\ 0 & 0 & 0.0999 \end{bmatrix} \begin{bmatrix} 10 \\ 20 \\ 40 \end{bmatrix} \newline
= \begin{bmatrix} 9.999 \\ 19.998 \\ 39.996 \\ 0.999 \\ 1.998 \\ 3.996 \end{bmatrix}
$$

```cpp
dst[0] = this->X(0, 0); // 9.999
dst[1] = this->X(1, 0); // 19.998
dst[2] = this->X(2, 0); // 39.996
dst[3] = this->X(3, 0); // 0.999
dst[4] = this->X(4, 0); // 1.998
dst[5] = this->X(5, 0); // 3.996
```
$$
\begin{bmatrix} x \\ y \\ z \\ v_x \\ v_y \\ v_z \end{bmatrix} = \begin{bmatrix} 9.999 \\ 19.998 \\ 39.996 \\ 0.999 \\ 1.998 \\ 3.996 \end{bmatrix}
$$
Comparing to results from kalman filter implementation in C++:
```cpp
Kalman Filter Result:
X: 9.99505
Y: 19.9901
Z: 39.9802
X Velocity: 0.989599
Y Velocity: 1.9792
Z Velocity: 3.9584
```

This yeilds a predicted x of 9.99505, a predicted y of 19.9901, and a predicted z of 39.9802. This is very close to the actual values of 10, 20, and 40. Following the assumption of a constant velocity, 10mm / 1 sec = 0.1 mm in 0.1 seconds. We can see that our predicted x velocity is 0.989599. This indicated the predicted velocity is very close to the actual velocity of the model we assumed.

0 comments on commit 74f2fd9

Please sign in to comment.