# 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 ${\displaystyle {\dot {z}}=f(z,u)}$ where ${\displaystyle z=(x,y,\theta )^{T}}$ and ${\displaystyle u=(v,\phi )^{T}}$. Use ${\displaystyle \phi =0}$ for simplicity.

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

${\displaystyle {\dot {P}}=AP+PA^{T}-PC^{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 ${\displaystyle A(t)}$ come from linearization of the observer: ${\displaystyle 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 ${\displaystyle {\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 ${\displaystyle 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 ${\displaystyle {\dot {z}}}$, the estimator dynamics ${\displaystyle {\dot {\hat {z}}}}$, and the Riccati Equation ${\displaystyle {\dot {P}}}$ simultaneously. This will allow you to simultaneously update the A matrix, P matrix, and L matrix.

--John