# Problem 3 - Is the A matrix zero?

**Q:** Is the A matrix zero in problem 3?

**A:** No it is not.

The problem dynamics are given as \( \dot{z} = f(z,u) \) where \( z = (x,y,\theta)^{T} \) and \( u=(v,\phi)^{T} \). Use \( \phi =0 \) for simplicity.

The estimator can be built directly from the system dynamics\[ \dot{\hat{z}} = f(\hat{z},u) + L(y - C \hat{z}) \]. As Kalman showed, the optimal observer gain is \( L= P(t)C^{T}R^{-1}(t) \), where \( P(t) \) comes from a solution of the Riccati equation

\( \dot{P} = AP + PA^{T} - P C^{T} R^{-1}CP + FQF^{T}\)

Note, there are no random disturbances affecting the system dynamics, so F=0 in these equations. The entries of matrix \( A(t) \) come from linearization of the observer\[ A = \left.\frac{\partial f}{\partial \hat{z}}\right|_{\hat{z}_{ref},u} \]. If you utilize the A matrix with the initial state of the observer with the Matlab lqe or kalman command, you will receive an error. This is because these Matlab functions assume \( \dot{P} = 0 \). This will not work in this problem due to its nonlinearity.

You will need to numerically integrate the Riccati equation to get \( P(t) \). Additionally, over time the observer state will also change, and the linearization A at time zero will not be valid for all time. Thus, your numerical integration will need to update the A matrix for each step of your iteration.

Suggestion: Utilize the same numerical integration call to integrate the original dynamics \( \dot{z} \), the estimator dynamics \( \dot{\hat{z}} \), and the Riccati Equation \( \dot{P} \) simultaneously. This will allow you to simultaneously update the A matrix, P matrix, and L matrix.