How to linearize this state space model and write it in discrete form?

Question

This might not be trivial nor short so in advance thank you all who read this in attempt to help. I'm building a Kalman filter in matlab and I'm fairly certain the software itself is working correctly because I tested it on some examples I found on the internet. Therefore my suspicion is that I made a mistake (either mathematical or conceptual) in the process of linearization and discretization of my model.

I need this Kalman filter to use it as an estimator of vehicle's mass (which is, obviously, unknown). The mass is modeled as a disturbance variable. This is the model of the system:

$$\dot{\mathbf x} = f\left(\mathbf x, u\right) \tag 1$$ $$ \begin{bmatrix} \dot{\tau}\\ \dot{v} \\ \dot{m} \end{bmatrix}=\begin{bmatrix}\frac{-1}{T}\tau+\frac{K}{T} u \\ \frac{\tau}{mr} \\ 0 \end{bmatrix} \tag 2 $$

where $\large \tau$ is torque, $\large v$ is the velocity (which is measured) and $\large m$ is mass. Other variables ($\large K, T, r$) are constants. $\large u$ is the input to the system.

Since there is a nonlinear term in the second row of the right-side matrix, I decided to linearize this system in order to write it in a typical state-space form. I did this by computing the Jacobian matrix which led me to the state-space form as follows. $$ \dot{\mathbf{x}}=\mathbf{A} \mathbf{x}+\mathbf{B}u \tag 3 $$ $$ \begin{bmatrix} \dot{\tau}\\ \dot{v} \\ \dot{m} \end{bmatrix} = \begin{bmatrix} \frac{-1}{T} & 0 & 0\\ \frac{1}{m_{op}r} & 0 & \frac{-\tau_{op}}{rm_{op}^{2}}\\ 0& 0 & 0 \end{bmatrix} \begin{bmatrix} \ \tau\\ v \\ m \end{bmatrix} + \begin{bmatrix} \frac{K}{T}\\0 \\ 0 \end{bmatrix} u \tag 4 $$

Note that I will be doing linearization separately for each step of the Kalman filter, so the variables with subscript op indicate "operating point". So those are constants, but different constants in each step.

What is left is to transform this linearized state-space model in it's discrete form. I did this using the first order Taylor expansions. $$ A_{d}=I+A \Delta T= \begin{bmatrix} 1-\frac{\Delta T}{T} & 0 &0 \\ \frac{\Delta T}{m_{op} r} & 1 & \frac{-\tau_{op} \Delta T}{m_{op}^{2} r}\\ 0& 0 & 1 \end{bmatrix} \tag 5 $$ $$ B_{d}=B \Delta T= \begin{bmatrix} \frac{K \Delta T}{T}\\0 \\ 0 \end{bmatrix} \tag 6 $$

Where $\large \Delta T$ is the discretization time. The C matrix is the same throughout since we are only measuring the second state (velocity), hence:

$$ C_{d}=C=\begin{bmatrix} 0 & 1 & 0 \end{bmatrix} \tag 7 $$

Finally we have the model in it's discrete form: $$ \mathbf{x}[k]=\begin{bmatrix} 1-\frac{\Delta T}{T} & 0 &0 \\ \frac{\Delta T}{m_{op} r} & 1 & \frac{-\tau_{op} \Delta T}{m_{op}^{2} r}\\ 0& 0 & 1 \end{bmatrix} \mathbf{x}[k-1]+\begin{bmatrix} \frac{K \Delta T}{T}\\0 \\ 0 \end{bmatrix} u[k-1] \tag 8 $$ $$ y[k]=\begin{bmatrix} 0 & 1 & 0 \end{bmatrix} \mathbf{x}[k] \tag 9 $$

So once again I would ask somebody to check if what I did is correct (conceptually and mathematically) because my suspicion is I made a mistake somewhere in these steps. When I plug this discrete state-space model in my code, the estimated vehicle mass slowly fades to 0. I checked the observability matrix, and it's rank is equal to 3 so this should be possible to estimate.

Answer 1

You have

$$ f\left(\mathbf x, u\right) = \begin{bmatrix}\frac{-1}{T}\tau+\frac{K}{T} u \\ \frac{\tau}{mr} \\ 0 \end{bmatrix} \tag a $$

From which you (eventually) derive

$$ \mathbf {A}_d=\begin{bmatrix} 1-\frac{\Delta T}{T} & 0 &0 \\ \frac{\Delta T}{m_{op} r} & 1 & \frac{-\tau_{op} \Delta T}{m_{op}^{2} r}\\ 0& 0 & 1 \end{bmatrix} \tag b $$

This part is -- mostly or entirely -- correct (more on that "mostly" later).

However, you're mistaking the function of the $\mathbf{A}_d$ in the EKF. $$\mathbf{x}_{k+1} = \mathbf{A}_d \mathbf{x}_k + \cdots$$ is not an accurate innovation function for $\mathbf{x}_k$. In fact, if you inspect $\mathbf{A}_d$, then you'll see that for $\tau_{op} = \tau_{k}$ and $m_{op} = m_{k}$, the estimated velocity does not change as a function of $\tau$ or $m$, where a quick look at $f(\mathbf x, u)$ shows that it is, indeed, a function of those two variables.

What $\mathbf{A}_d$ isn't is the basis of a linear innovation function for $\hat{\mathbf x}_k$. What $\mathbf{A}_d$ is, is the sensitivity of the actual nonlinear innovation of $\mathbf x$, i.e. $f$, to the parameters. So it's clearly wrong to estimate $\mathbf x$ -- but it has proven to be pretty good for estimating $\hat{\mathbf P}_k$.

Your estimation step should should use $f$ as the innovation function. A least-acceptable estimate would be $$\hat {\mathbf{x}}_{k+1} = \hat {\mathbf{x}}_{k} + \Delta T f\left(\hat {\mathbf{x}}_{k}, u\right). \tag c$$

(Note that you may be tempted to use the numerical solution of $$\dot{\hat{\mathbf x}} = f\left(\hat{\mathbf x}, u\right) \tag d$$ for $\hat{\mathbf x}\left( \Delta{T} (k + 1) \right)$. If you need to do that to retain accuracy, then you should revisit your decision to use the EKF over the unscented Kalman, or you should figure out how to increase your sampling frequency).

Where you use $\mathbf{A}_d$ is in predicting your a-priori covariance, i.e. $$\hat{\mathbf P}^-_{k+1} = \mathbf{A}_d \hat{\mathbf P}^+_k \mathbf{A}_d^T + \mathbf Q$$

So -- I think that's your main problem.

However, it is now later, and you are using $\tau_{op}$ and $m_{op}$ in your calculation of ${\mathbf A}_d$. If you truly mean that you are designing your filter by assuming a $\tau_{op}$ and an $m_{op}$, calculating a fixed ${\mathbf A}_d$, and using it throughout, then your technique will only work for slight variations of $\tau$ and $m$ around their operating points (I would guess that as much as $\pm 10\%$ would still sort of work, but that is most definitely a guess, subject to verification).

If your estimates for $\tau$ and $m$ vary significantly, you should calculate ${\mathbf A}_d$ at each time step. Instead of (b) you should use

$$ \mathbf {A}_{d, k} = \mathbf J f(\hat{\mathbf x}, u) = \begin{bmatrix} 1-\frac{\Delta T}{T} & 0 &0 \\ \frac{\Delta T}{m_k r} & 1 & \frac{-\tau_k \Delta T}{m_k^{2} r}\\ 0& 0 & 1 \end{bmatrix} \tag e $$

What you are doing here is using a common dodge in solving problems in nonlinear systems. Because the Kalman filter works for any linear system, even if it's time-varying, you use that time-varying form (through your calculation of $A_d$ as the step-by-step Jacobian of $f$), you just don't "tell" the computation that you're "really" using the nonlinear function for the state evolution.