1
$\begingroup$

The question is related to the implementation of a discrete kalman filter given a description of the system model in continuous time.

I will give an example. Suppose we have a mass, spring and damper system as below:

Mass, spring and damper system

The differential equation describing this system is:

$$\ddot{x} = -\frac{k}{m}x - \frac{b}{m}\dot{x} + \frac{1}{m}F$$

Therefore, if the system states are:

$$\begin{bmatrix} x \\ \dot{x} \end{bmatrix}$$

Then, the state space matrices are (assuming we can only measure position):

$$A = \begin{bmatrix} 0 & 1 \\ -\frac{k}{m} & -\frac{b}{m} \end{bmatrix}$$

$$B = \begin{bmatrix} 0 \\ \frac{1}{m} \end{bmatrix}$$

$$C = \begin{bmatrix} 1 & 0\\ \end{bmatrix}$$

$$D = \begin{bmatrix} 0 \end{bmatrix}$$

I would like to implement a discrete kalman filter for this system, following the regular discrete system convention:

$$\hat{x}_{k} = A\hat{x}_{k-1} + Bu_{k} + w_{k}$$

$$z_{k} = C\hat{x}_{k} + v_{k}$$

Of course, the A, B, C and D matrices of the continuous system are not the same ones as for the discrete system.

I have tried to convert the continuous system to a discrete one (using ZOH, see also here) but it didn't seem to work.

Any ideas on how to tackle such a problem? An example would also be nice.

$\endgroup$
  • 1
    $\begingroup$ just about any text book that covers linear systems by state variables shows how to go from the differential equations to the state transition equations using Euler integration. typically most text books have solutions that you can work out by hand but it’s typical to use a numerical approach. for linear and nonlinear state variables, there are known solutions. there is nothing state of the art here. $\endgroup$ – Stanley Pawlukiewicz Jun 19 at 12:28
  • $\begingroup$ @StanleyPawlukiewicz state of what ? Here Kalman filter has the states of position and velocity ? I don't know about art state however ? Are you kidding like me ? We are even solving homeworks here... Your comment was terrible... Yet I absolutely agree on the fact that student must display / prove his efforts before asking for a complete ! solution... $\endgroup$ – Fat32 Jun 19 at 20:56
  • $\begingroup$ @Fat32 , state-of-the-art is a colloquialism . perhaps “google state transition equations” would have been more informative. Oppenheim and Schaefer did cover state variables in their 1975 Orange first edition. Somehow state variables seem to have dropped out of DSP. $\endgroup$ – Stanley Pawlukiewicz Jun 19 at 21:14
  • $\begingroup$ @StanleyPawlukiewicz that (mine) was a joke... :-) $\endgroup$ – Fat32 Jun 19 at 21:21
  • $\begingroup$ @Fat32 you might want to keep your day job $\endgroup$ – Stanley Pawlukiewicz Jun 19 at 21:24
2
$\begingroup$

The mass-spring-damper combination is an LTI system described by the following continuous-time linear differential equation

$$ \ddot{x} = - \frac{k}{m} x - \frac{b}{m} \dot{x} + \frac{1}{m} u $$

where $u$ is the deterministic input force (N), $k$ is the spring constant (N/m) and $b$ is the damping coefficient (N.s/m).

Assuming two states as the position $x(t)$ and the velocity $\dot{x}(t)$; $X = [x , \dot{x}]^T$, and assuming a single measurement as the position $x(t)$ and assuming no process noise, and scalar deterministic input $u$ only added to the process equation, then the continuous-time linear dynamic system process and measurement equations for the Kalman filter can be written as

$$ \dot{X} = F X + G u $$ $$ y = H X + v $$

where $$ F = \begin{bmatrix} 0 & 1 \\ -\frac{k}{m} & -\frac{b}{m} \end{bmatrix} $$

is the system dynamics matrix,

$$ G = \begin{bmatrix} 0 \\ 1/m \end{bmatrix} $$ is the deterministic input matrix,

$$ H = \begin{bmatrix} 1 & 0 \end{bmatrix} $$ is the measurement matrix, $v(t)$ is the measurement noise.

The first step in the derivation of the discrete Kalman filter is the discretization of the associated matrices by first finding the fundamental matrix $\Phi(t)$ which is given by :

$$ \Phi(t) = \mathcal{L}^{-1} \{ (s I - F)^{-1} \} $$

where $s$ is the complex variable of the Laplace transform.

For the given $F$, the fundamental matrix is found to be:

$$ \Phi(t) = e^{\frac{-b}{2m}t} \begin{bmatrix} \cos(\omega t) + \frac{b}{2m\omega } \sin(\omega t) & \sin(\omega t) / \omega \\ -\frac{k}{m\omega } \sin(\omega t) & \cos(\omega t) - \frac{b}{2m\omega } \sin(\omega t) \end{bmatrix}\\ $$

where $\omega = \frac{\sqrt{4mk-b^2}}{2m}$.

Then the discrete fundamental matrix of the Kalman filter is found from $\Phi_k = \Phi(T)$ where $T$ is the sampling period.

$$ \Phi_k = e^{\frac{-b}{2m}T} \begin{bmatrix} \cos(\omega T) + \frac{b}{2m\omega } \sin(\omega T) & \sin(\omega T) / \omega \\ -\frac{k}{m\omega } \sin(\omega T) & \cos(\omega T) - \frac{b}{2m\omega } \sin(\omega T) \end{bmatrix}\\ $$

We should also find the discrete versions of $H$ and $G$ which are found as $H_k = H = [1 , 0]$ and

$$G_k = \int_0^T \Phi(t) G dt .$$

By the help of a symbolic integration package (such as provided by MATLAB) one can compute the discrete deterministic input matrix $G_k$ as

$$ G_k = \frac{e^{aT}}{m (a^2+w^2)} \begin{bmatrix} \frac{a}{\omega} \sin(\omega T)-\cos(\omega T) + e^{-aT} \\ a (1-\omega) (\cos(\omega T)-1) + (a^2+ \omega) \sin(\omega T) \end{bmatrix}\\ $$

where $ a = -b/2m $ and $\omega = \sqrt{4km-b^2}/2m$ as before.

After having found all the necessary discrete matrices $\Phi_k$, $G_k$, $H_k$ we can setup the discrete Kalman process and measurement equations through which the recursive solution can be applied.

$$X[n] = \Phi_k X[n-1] + G_k u[n]$$ $$ y[n] = H_k X[n] + v[n]$$

where $u[n]$,$v[n]$ and $y[n]$ are sampled versions of deterministic input, measurement noise and measurement respectively, and $X[n]$ is the sampled version of the state vector.

Then you will apply the Kalman recursion to estimate the states. Below are some simulation results from a MATLAB implementation of the associated Kalman filter. In the simulation, the true signal is generated by solving the associated ODE with a simple Runge-Kutta numerical method. Eventhough it's not required for this LTI system whose analytical solution is already available, it'll be helpful to use a numerical method later if more complicated (nonlinear) differential equations are encountered.

enter image description here

enter image description here

enter image description here

In this simulation, the model was exact. And therefore it's a little trivial. For more realistic applications, one should either replace the linear differential equation with more realistic nonlinear ones hence change the model and/or apply process noise to compansate for the lack of exact process model information.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.