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

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.

• It's worrying me slightly that your original $A$ matrix is rank 2. Usually the extended Kalman filter doesn't use the Taylor series expansion, it just takes derivatives.. A little late for me to dig now; I'll see if I have time tomorrow to have a go.
– Peter K.
Oct 8 at 1:26
• I took the liberty of adding equation numbers, and equation (1), so that I could ask: when you compute your actual $\mathbf {x}(k)$, are you using (8), or are you using $\mathbf {x}(k) = \mathbf{x}(k-1) + \Delta T f \left( \mathbf {x}(k-1), u \right)$? Oct 8 at 4:58
• Boy am I glad you guys reformatted the math. Now it's recognizable. Oct 8 at 5:00
• @TimWescott I use the equation (8) to compute x(k) in the prediction part of the Kalman filter algo Oct 8 at 10:56
• I noticed that my torque estimates are quite bad as well, they are scaled by a factor. So I tried to use tau/2 instead of tau in order to calculate my Ad matrix in each step and the results of the mass estimation seem to improve. So I reckon my mass is estimated badly because torque is estimated badly? Oct 8 at 12:30

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.

• This shouldn't be your problem -- but just looking at the math, if you choose to estimate $1/m$ instead of $m$, you avoid the singularity at $m = 0$, as well as reducing the curvature of the Jacobian of $f$ for unrealistically low estimates of $m$. Oct 8 at 15:13
• Thank you. If I understand correctly, linearizing the state-space model is used just to calculate the covariance matrix, while innovation is calculated using the nonlinear model? If so, I'm not sure how to calculate the innovation since my nonlinear model is in continuous form. I tried to discretize it just logically $$\begin{bmatrix}\tau(k) \\ v(k) \\ m(k) \end{bmatrix} = \begin{bmatrix} \tau(k-1)+(\frac{-\tau(k-1)}{T}+\frac{K}{T}u(k-1))\triangle T\\ v(k-1)+(\frac{\tau(k-1)}{m(k-1)r})\triangle T \\ m(k-1) \end{bmatrix}$$ but now my mass estimates starts raising. Oct 8 at 23:11
• That is, indeed, my suggestion for your first cut at making it discrete time. Does the mass estimate just start rising, or does it appear to rise without bound? Oct 9 at 2:28
• It appears to start settling at a wrong value. I tried a few configurations of different R and Q matrices but it looks the same - ibb.co/S7cHbPT This is my main loop of the EKF - ibb.co/ZYgvc9x You did answer my question so I will accept your answer, but if you have an idea why the estimation still isn't good I would appreciate your input. Oct 9 at 9:38