|
FhSim
3.1.0
Marine systems simulation
|
|
|
FhSim
3.1.0
Marine systems simulation
|
Fusion of measurements into the model simulation is useful for dynamically adapting the numeric simulation to e.g. real-world observations. FhSim facilitates state and parameter estimation using an Unscented Kalman Filter (UKF). Please read this documentation if you intend to use the built-in Kalman filtering in FhSim and/or are not familiar with Kalman filtering. To jump right to the core, see Implementing the KalmanObject.
This section will give a quick and dirty overview of the equations in a Kalman filter and UKF in particular. The reader is encouraged to consult resources elsewhere if a more in-depth introduction is sought.
Suppose we have a state vector \(x(t) \in \mathbb{R}^n\) with an ordinary differential equation on the form
\[\dot x = f(x,t) + w(t),\]
where \(w(t)\) is process noise. The system output \( y(t) \in \mathbb{R}^m\) is obtained at regular time points with period \(\Delta t\). The \(k\)th measurement occurs at \(t_k = t_0 + k\Delta t\) and is on the form
\[y_k := y(t_k) = h(x,t_k) + v(t_k),\]
where \(v(t)\) is measurement noise.
The objective of the Kalman filter is to provide an estimate of the state vector based on the available information. Information of relevance include initial conditions and the measurement sequence. Moreover, the Kalman filter exploits the statistical information about the process and measurement noise vectors together with the uncertainty of the state vector in determining the updated state vector. Let \( z \sim N(\mu,\Sigma) \) indicate that \(z \in \mathbb{R}^p\) is a multivariate vector with mean \(\mu\) and covariance matrix \(\Sigma \in \mathbb{R}^{p \times p}\), which is positive definite. Now, let
\begin{align*} w &\sim N(0,Q_c),\\ v &\sim N(0,R), \end{align*}
such that \(Q_c\) and \(R\) are the process and measurement covariance matrices, respectively. The diagonal terms of these matrices are the variance of the corresponding state derivative or measurement. The uncertainty of the state vector is taken as the covariance of the estimation error:
\[ P := E[(x - E[x]])(x - E[x])^T, \]
where \( E[\cdot]\) is the expectation operator. The estimation error covariance matrix \(P(t)\) (and the diagonal entries in particular) is a measure of how uncertain the state estimate is at a particular time point.
The Kalman filter consists of three phases: initialization, time update, and measurement update. The last two phases are repeated for each new measurement, see diagram illustration.
The initialization is done before the estimation itself begins. This includes setting the initial conditions of the state vector and other parameters, just as in a regular simulation in FhSim. The initial condition of the state vector \(\hat x(t_0) = \hat x_0\) is the best guess of the real state. With Kalman filtering the user also has to provide covariance matrices \(P(t_0),\,Q,\,R\).
The time update is an open-loop simulation from one time point \(t_{k-1}\) to \(t_k\), where a new measurement becomes available. For the Extended Kalman filter this is the solution to the initial value problem \( t \in [t_{k-1},t_k]\)
\[ \dot{\hat x}^-(t) = f(\hat x,t),\, \hat x(t_{k-1}) = \hat x_{k-1},\]
where \(\hat x_k^- =\hat x^-(t_k)\) is the so-called a priori estimate. This estimate is has not yet incorporated the measurement \(y_k\).
The estimation error covariance matrix also has to be updated. For the Extended Kalman filter this involves linearizing the system dynamics and solving a matrix differential equation, which includes of the previous time step's covariances \( P_{k-1}\) and \(Q\). Details will not be covered here.
In the measurement phase the a priori estimates are updated by incorporating the information that the measurement \(y_k\) provides. The equations for this update are
\begin{align*} \hat x_k &= \hat x_k^- + K_k(y_k - \hat y_k)\\ P_k &= P_k^- - K_k P_y K_k^T, \end{align*}
where \(K_k\) is the Kalman gain matrix, which is based on the system uncertainties, and \( \hat y_k \) is the estimated measurement. \( \hat x_k \) is often called the a posteriori state estimate.
The Extended Kalman filter (EKF) relies on first-order approximations of the mean and third-order approximations of the covariances when calculating the state estimates. These calculations require Jacobian information, which may be difficult to calculate/approximate accurately. The Unscented Kalman filter circumvent the Jacobian requirement by using so-called unscented transformations. This approach is based on two ideas:
When the unscented transformations are used to propagate the means and covariances instead of its linearized counterparts of the EKF, it is called the Unscented Kalman filter. The method enjoys the benefit of tackling highly nonlinear systems with discontinuities better than for instance the EKF. This approach was first proposed in Julier, S. J., Uhlmann, J. K., and Durrant-Whyte, H. F. (1995). "A new approach for filtering nonlinear systems". In American Control Conference, pages 1628-1632, Seattle, Washington, USA.
An unscented transformation is basically the function evaluation of a set of input vectors through a nonlinear function. The outputs of these function evaluations are used to approximate the statistical properties, such as mean and covariance, of the nonlinear transformation. By appropriately choosing the input vectors and the weighting when combining the outputs, we obtain an approximation of the probability density function of the transformation at hand.
Suppose that we have a nonlinear function \( y: R^n \to R^n\), whose domain has known mean and covariance:
\begin{align*} y &= h(x)\\ x &\sim N(\hat x, P_x). \end{align*}
Let \(\{x^{(i)}\}_0^{2n}\) be a sequence of carefully chosen sigma points. We perform a nonlinear transformation \(h(\cdot)\) of all these points to get a mapped sequence \(\{y^{(i)}\}_0^{2n}\). Weighted sums and sample covariance of these outputs provide us with an approximated mean and covariance of the mapping \( h(\cdot)\), namely \( \hat y\) and \(\hat P_y\). This procedure is illustrated for a two-dimensional mapping in the figure below.
In FhSim the UKF is implemented based on the publication http://www.dx.doi.org/10.1109/ICASSP.2001.940586. Please consult this publication for details on the equations involved. We have implemented both the normal and the square root variant, but due to numerical issues (or implementation errors) with the latter, we currently use the normal one.
The Kalman filter implementation make use of three central classes in FhSim: CKalmanIntegrator, estimator::KalmanFilter, and KalmanObject. In addition, some supporting data structures are used.
CKalmanIntegrator utilizes a user-specified fixed-step COdeSolver to step the state forward in time. It also assembles covariance matrices and measurement vector contributions from each KalmanObject. This information is used by the estimator::KalmanFilter (and its child estimator::UKF) to solve the Kalman filter equations and update the state estimate vector and other relevant data structures as described in the above mentioned paper. The two most costly operations in the filter are to simulate the ensemble of sigma points forward in time and the solution to a matrix square root. The ensemble update involves the solution to \(2n+1\) IVPs. In contrast, a pure simulation only needs to solve one IVP at any given time. Large-scale systems are thus expected to struggle for real-time performance (Note: Profiling of the Kalman filter may reveal sources for improvement).
The user usually only has to deal with the KalmanObject interface to use the Kalman filter functionality in their own simulations.
Suppose you want to estimate some process using a Kalman filter. If you already have implemented a SimObject, you are in luck, because then you only have to inherit from this object in addition to the KalmanObject interface. In broad strokes, once a SimObject has been extended with the KalmanObject interface, the input file for a KalmanObject is not very different from a regular SimObject. Only some new connections and/or input parameters may need to be defined.
The implementation defines a KalmanObject interface class. This class declares several virtual functions that need to be defined in order to enable Kalman filtering. We provide two examples to get you started.
The pendulum example gives you a cut-to-the-bone introduction of what you need to do to get started, whereas the case study is a bit more involved, providing some more guidelines and details on how to use Kalman filtering in your project, faced with some additional challenges.
_i, such as Euler_i and RK45_i, as used throughout this tutorial — have that wrapper. The SUNDIALS methods (BDF, Adams, DIRK, ERK) do not: a model containing KalmanObjects will be integrated under them, but the objects will not be estimated. FhSim logs a warning when that happens.Suppose we have a cable connected to a fixed point in one end and a sphere mass in the other end, both points submerged in water. The setup is under the influence of gravity, buoyancy, and some unknown, slowly-varying current. Every \(\Delta t\) seconds a noisy measurement of the sphere position is obtained. Our objective is to estimate the cable's state vector and the water current. We proceed by modeling the problem as a simple hydrodynamic cable and ball, each component modeled in their own SimObject.
Let the inertial reference frame be fixed to north, east, and down directions with coordinates ordered as \((N,E,D)\). Define \(v_c(t) \in \mathbb{R}^3\) as the slowly-varying uniform current.
Define \(p(t) \in \mathbb{R}^3\) and \(v(t) \in \mathbb{R}^3\) as position and velocity of the ball, respectively. The ordinary differential equations for the hydrodynamic ball are
\begin{align*} \dot p &= v,\\ \dot v &= \frac{F(p,v,t)}{m}, \end{align*}
where \(F\) is the force acting on the ball and \(m\) is the ball's mass. The forces acting on the ball are gravity, buoyancy \(B\), drag \(D\), and other external forces: \(F(p,v,t) = mg - B + D + f_{ext}(p,v,t)\), where \(g = (0,0,9.81)\). We use that the ball volume is \(\frac{4}{3}\pi r^3\) and assume that the ball is completely submerged, so \( B = \frac{\rho_w}{\rho_b}mg\), with \(\rho_w,\,\rho_b\) being the water and ball density, respectively. The drag on the ball is modelled as Morison drag \( D = -0.5 \rho_w \pi r^2 C_d (v-v_c)\|v-v_c\|_2\), where \(r\) is the ball radius and \(C_d\) is the drag coefficient (the latter is set to \(0.5\)). After simplifying the equations we arrive at the following force function
\[ F = mg\left(1 - \frac{\rho_w}{\rho_b}\right) - m\frac{3C_d}{8r}\frac{\rho_w}{\rho_b}v_r \|v_r\|_2 + f_{ext}(p,v,t), \]
where \(v_r := v - v_c\) is the relative current.
Define \( x_b(t) := [p^T\, v^T]^T \in \mathbb R^6\) as the state vector for the ball. The compact description for the ball dynamics is
\begin{align*} \dot x_b &= f_b(x_b,t),\\ y_b(t) &= p(t) + v(t), \end{align*}
where \(y_b(t)\) is the noisy measurement of the ball's position.
We model a simple mass-lumped cable that consists of point masses connected by spring and damper forces. Like the ball, the cable is under the influence of gravity, buoyancy, and Morison drag. The characteristics of the cable are given in the table below.
| Description | Symbol | Numeric value (if const.) |
|---|---|---|
| Length | \(L\) | - |
| Diameter | \(d\) | - |
| Density | \(\rho_c\) | - |
| Mass | \(M\) | \(\rho_c \pi \left(\frac{d}{2}\right)^2 L\) |
| Elastic modulus | \(E\) | - |
| Cross-section drag coef. | \(C_d\) | 1.1 |
| Streamlined drag coef. | \(C_t\) | 0.1 |
We discretize the cable into \(n+1\) uniform line segments, so that there are \(n\) point masses excluding the end points. Each point has mass \(m_i = \frac{M}{n}\). Let \( i \in \{1,2,\dots,n\} =: \mathbb{I}_n\) be the indices of the points. The indices \(i = 0\) and \(i = n+1\) are the end points. For each \(i \in \mathbb{I}_n\) let \(p_i(t),v_i(t) \in \mathbb R^3\) be the position and velocity with dynamics
\begin{align*} \dot p_i &= v_i,\\ \dot v_i &= \frac{F_i(p_{i-1},p_i,p_{i+1},v_i,t)}{m}, \end{align*}
where \(F_i\) is the forces acting on the \(i\)th point mass. The forces are
\[ F_i = m_ig\left(1 - \frac{\rho_w}{\rho_c}\right) + D_i + \Lambda_i + \Delta_i, \]
where \(\rho_w\) is water density, \(D_i\) is drag, \(\Lambda_i\) is spring forces, and \(\Delta_i\) is damper forces. In the following we will define the drag, spring, and damper forces.
Each point mass has two line segments connected to it. From each of these two line segments there will be a contribution to the drag that acts on the point. We take half a line segment's length and calculate the drag on this cylinder. Let us continue by defining some expressions that make the exposition simpler. Let \(k,l \in \mathbb I_n \cap \{0,n+1\}, k\neq l\).
\begin{align*} \bar p (k,l) &:= \frac{p_l - p_k}{\|p_l - p_k\|_2},\\ v(k,v_c) &:= v_k - v_c,\\ v_{k,l}^\| &:= \left(v(k,v_c)\cdot \bar p(k,l)\right)\bar p(k,l),\\ v_{k,l}^\perp &:= v(k,v_c) - v_{k,l}^\|, \end{align*}
where \(v_c(t) \in \mathbb R^3\) is the water current and \(\cdot\) is the dot product. \(\bar p\) is the unit vector pointing from \( p_k\) to \(p_l\). \( v(k,v_c)\) is the velocity of \(p_k\) relative to the current, whereas \(v_{k,l}^\|\) and \(v_{k,l}^\perp\) are the decomposition into a velocity parallel to and perpendicular to the corresponding line segment, respectively.
The drag on line segment \((k,l)\) acting on point \(k\) is
\[ D(k,l) = -\frac{1}{2}\rho_w \underbrace{d 0.5\|p_l - p_k\|_2}_{A} \|v(k,v_c)\|_2\left(C_d v_{k,l}^\perp + C_t v_{k,l}^\| \right). \]
The total drag on point \(i \in \mathbb I_n\) is
\[ D_i = D(i,i+1) + D(i,i-1). \]
The spring forces acting on a point mass come from the line segments connected to the neighboring points. The relaxed length of line segment \((k,l)\) is \( L_0(k,l) = \frac{L}{n+1} := L_0\). We assume linear spring forces under extension only (no compression forces). The spring force acting on point \(k\) due to line segment \((k,l)\) is
\[ \Lambda(k,l) = \begin{cases} \overbrace{E\pi \frac{d^2}{4} \frac{1}{L_0}}^{\lambda}\left(\|p_l - p_k\|_2 - L_0 \right)\bar p(k,l) &\mbox{if } \|p_l - p_k\|_2 \geq L_0\\ 0 & \mbox{otherwise } \end{cases} \]
The spring forces acting on point \(i \in \mathbb I_n\) are
\[ \Lambda_i = \Lambda(i,i+1) + \Lambda(i,i-1). \]
The damping forces on a point in the cable also depend on the neighboring points. We denote the damping on point \(k\) from point \(l\) as
\[ \Delta(k,l) = 2\sqrt{\lambda m_k}\left((v_l - v_k) \cdot \bar p(k,l) \right)\bar p(k,l), \]
where \( k \) was defined when discussing spring forces.
The damping forces acting on point \(i\in \mathbb I_n\) are
\[ \Delta_i= \Delta(i,i+1) + \Delta(i,i-1). \]
We have now described all forces acting on the cable. Putting it all together and detailing the compact description are outside the scope of this case study. We simply state that there exist such an equation. Define \( x_c(t) = [p_1^T\, v_1^T\, \cdots\, p_n^T\, v_n^T]^T \in \mathbb R^{6n}\) as the cable state vector with compact description
\begin{align*} \dot x_c &= f_c(x_c,t), \end{align*}
where the end points and water current are considered time-varying expressions.
Both the hydrodynamic ball and cable have been implemented as SimObjects. Their documentation and can be found in the fhsim_base under Example/HydroBall and Example/HydroCable. We assume that the setup is under the influence of a current, which consists of sine-wave with bias in north direction, and constant current in east and down directions.
\[ v_c(t) = \begin{bmatrix}a\sin(\omega t) + b\\c\\d\end{bmatrix}, \]
where \(a,\, \omega,\, b,\, c,\, d\) are sine-wave amplitude, angular velocity, bias, and constants, respectively.
We use the following input files for simulation of a ball connected to a cable.
Simulation.xml:
Now, suppose the simulation described in A sphere attached to a cable submerged in water in the section above is "a real-world process" and we do not know neither the current, nor the states of the ball or cable. We do, however, get a noisy measurement of the ball's position every \(\Delta t \) seconds. Our objective is to estimate the cable's state vector and the water current.
The simulator setup is as follows. We start a FhSim process and denote this the real-world simulation. Usually, information about the real-world process is collected through various sensors and peripheral devices with various sampling rates and noise. To mimic this behavior we:
We add the following code snippet at appropriate locations in Simulation.xml above (consult documentation for respective SimObjects for details)
Simulation.xml now simulates the real world process, obtains measurements at regular intervals, and sends them via DDS to be accessed by the estimator. The estimator, which is another FhSim process, employs KalmanObjects and incorporates the received measurements of the simulated real world. We continue by first describing the estimator setup and then detailing the necessary KalmanObject implementations.
When estimating the process at hand we need mathematical descriptions of the objects involved. Luckily, we already have implemented SimObjects that capture most of the phenomena at hand. What we do not know, though, is the time-varying current. A common assumption when estimating parameters is to assume that they are constant or slowly-varying. In such a case, one can describe the process with "zero-dynamics" as follows. Suppose \( p(t) \in \mathbb{R}^m\) is an parameter vector with dynamics
\[ \dot p = 0 + w_p(t), \]
where \(w_p(t)\) is process noise. The initial conditions for this differential equation are the best guesses of the parameters. We use this description for the current in the estimator.
The involved SimObjects have been extended to KalmanObjects and the file below serves as the input file for the estimator.
Estimation.xml:
The contents of Estimator.xml is quite similar to Simulator.xml, with some extensions.
HMin; in v3 examples above it is represented with StepControl StepMin. In Estimator.xml above, the major time step is 0.5 s, so there are 2 500 integrator steps per measurement update. The major time step \( \Delta t\) must be an integer multiple of the step size.Let's proceed with the Kalman extension of the Example/HydroBall. Consider below the declaration of KalmanHydroBall that has the KalmanObject extension.
Notice that the class inherits from both HydroBall and KalmanObject, so we must implement the six pure-virtual functions of KalmanObject.
Below we have included the definition for each function of KalmanHydroBall. We will not explain every line of code, since it is (by some) deemed self-explanatory.
Implementation of Kalman/Example/HydroBall:
After following Create a new SimObject, KalmanHydroBall is accessible for use with FhSim.
The KalmanHydroCable is implemented in a similar fashion as the KalmanHydroBall, so the code will not be presented here. Some code will be given to highlight important points.
Excerpt of Kalman/Example/HydroCable:
When the number of internal states depends on input parameters, the implementer must take care so that the dimensions of belonging matrices in KalmanObject scales accordingly.
One can of course implement a SimObject with KalmanObject interface directly, where both the SimObject and KalmanObject interface are implemented simultaneously:
See Running the simulator and estimator example(s) on how to run the code.
The above example has been run and below you can se the measured and estimated position of the ball. You can also see the true and estimated current in the three lower subplots.
Using position measurements of the ball, we are able to approximate the current that influences the displacement of the ball. The estimates are not very good, and there may be several reasons for this:
It should be pointed out that we use identical numerical model descriptions for the simulator and estimator. This is not the case when filtering real-world measurements, and may be reflected in the estimator performance.
Consider a simple pendulum with angle \(\theta\) with respect to the z-axis of a north, east, and down reference frame, and angular velocity \(\omega\)
\begin{align*} \dot \theta &= \omega\\ \dot \omega &= -\frac{g}{l}\sin \theta, \end{align*}
where \(g\) is standard gravity and \(l\) is pendulum arm length.
Suppose you want to estimate its movement, given an angle measurement every \(\Delta t \) seconds. The pendulum is implemented in FhSim as the class Pendulum (Example/Pendulum). To implement the KalmanObject interface on this class, consider the following class declaration, which inherits from both Pendulum and KalmanObject:
This code shows the declarations of the virtual function from the KalmanObject interface that must be implemented.
The corresponding definitions for the above code is
To run a simulation using this code, we define two input files: one for a simulator, which emulates the real world, and an estimator, which tries to mimic the real world, given angle measurements on a regular interval. The measurement is transmitted using a Data Distributive Service (DDS) SimObject.
The input file for Simulator.xml is
For Estimator.xml we have
StepMin="0.5" in the v3 example above (legacy XML used HMin). Two separate command windows are needed to run the simulator and estimator setup. Both processes must advance at the same speed, so both are paced against the wall clock. In the snippets below they run at twice real time.
FhSim 3 has no separate real-time executable. Real time is a pacing mode the ordinary executables apply when a simulation speed is set. There are two ways to set it:
-s / --simulation-speed on the command line. This flag exists on FhVis only, and only in a build with visualization enabled.simulationSpeed as an attribute of the <SIMULATION> element in the input file. This works for FhSim as well as FhVis. The command-line flag overrides the XML value.With FhVis:
Without visualization, put the speed in each input file instead:
and run:
We can make a batch script that starts the two processes simultaneously. This is useful if one wants to visualize the states etc. using some visualization software.
RunBoth.bat (windows only):
Each line in RunBoth.bat is equivalent of performing the following: open a new command window, change directory into the one specified, and then run FhVis with the input parameters.
fhsim_extras.FhVis --simulation-speed \(k\) (or the simulationSpeed attribute of <SIMULATION>), with \(0 < k \), where \(k=1\) is execution in real time.
1.9.8