Filters#
This page is an introduction to the filters module. For the different filter objects themselves, go to Filters.
The filters module in c4dynamics provides a collection of
classes and functions for implementing various types of filters commonly used in control
systems and state estimation.
Background Material#
The background material and the sections concerning the particular filters are based on sources in references [AG] [SD] [ZP].
System Model#
State-space representation is a mathematical model of a physical system represented as a set of input, output, and state variables related by first-order differential (or difference) equations.
The state vector \(X\) of a state-space model provides a snapshot of the system’s current condition,
encapsulating all the variables necessary to describe its future behavior given the input.
(In c4dynamics the state vector is a fundamental data structure,
represented by the class state)
and a snapshot of its values is provided by
the property state.X).
When the coefficients of the state variables in the equations are constant, the state model represents a linear system (LTI, linear time invariant). If the coefficients are linear functions of time, the system is considered linear time varying. Otherwise, the system is nonlinear.
Nonlinear Systems#
All systems are naturally nonlinear. When an equilibrium point representing the major operation part of the system can be found, then a linearization is performed about this point, and the system is regarded linear. When such a point cannot be easily found, more advanced approaches have to be considered to analyze and manipulate the system. Such an approach is the extended Kalman filter.
A nonlinear system is described by:
Where:
\(f(\cdot)\) is an arbitrary vector-valued function representing the system process
\(x\) is the system state vector
\(t\) is a time variable
\(u\) is the system input signal
\(\omega\) is the process uncertainty with covariance matrix \(Q_c\)
\(y\) is the system output vector (the measure)
\(h(\cdot)\) is an arbitrary vector-valued function representing the measurement equations
\(\nu\) is the measure noise with covariance matrix \(R_c\)
\(x_0\) is a vector of initial conditions
The noise processes \(\omega(t)\) and \(\nu(t)\) are white, zero-mean, uncorrelated, and have known covariance matrices \(Q_c\) and \(R_c\), respectively:
Where:
\(\omega\) is the process uncertainty with covariance matrix \(Q_c\)
\(\nu\) is the measure noise with covariance matrix \(R_c\)
\(Q_c\) is the process covariance matrix
\(R_c\) is the measurement covariance matrix
\(\sim\) is the distribution operator. \(\sim (\mu, \sigma)\) means a normal distribution with mean \(\mu\) and standard deviation \(\sigma\)
\(E(\cdot)\) is the expectation operator
\(\delta(\cdot)\) is the Dirac delta function (\(\delta(t) = \infty\) if \(t = 0\), and \(\delta(t) = 0\) if \(t \neq 0\))
superscript T is the transpose operator
Linearization#
A linear Kalman filter has a significant advantage in terms of simplicity and computing resources, but much more importantly, the System Covariance in a linear Kalman provides exact predictions of the errors in the state estimates. The extended Kalman filter offers no such guarantees. Therefore it is always a good practice to start by an attempt to linearize the system.
The linearized model of system (1) around a nominal trajectory \(x_n\) is given by [MZ]:
Where:
\(\Delta{x}\) is the linear approximation of a small deviation of the state \(x\) from the nominal trajectory
\(\Delta{u}\) is the linear approximation of a small deviation of the input control \(u\) from the nominal trajectory
\(\omega\) is the process uncertainty
\(\Delta{\nu}\) is the linear approximation of a small deviation of the noise \(\nu\) from the nominal trajectory
\({\partial{f} \over \partial{i}}\bigg|_{x_n, u_n}\) is the partial derivative of \(f\) with respect to \(i (i = x\) or \(u)\) substituted by the nominal point \({x_n, u_n}\)
\({\partial{h} \over \partial{x}}\bigg|_{x_n}\) is the partial derivative of \(h\) with respect to \(x\), substituted by the nominal point \({x_n}\)
\(y\) is the system output vector (the measure)
\(x_0\) is a vector of initial conditions
Let’s denote:
Finally the linear model of system (1) is:
Where:
\(A\) is the process dynamics matrix
\(x\) is the system state vector
\(b\) is the process input matrix
\(u\) is the system input signal
\(\omega\) is the process uncertainty with covariance matrix \(Q_c\)
\(y\) is the system output vector (the measure)
\(C\) is the output matrix
\(\nu\) is the measure noise with covariance matrix \(R_c\)
\(x_0\) is a vector of initial conditions
\(Q_c\) is the process covariance matrix
\(R_c\) is the measurement covariance matrix
Sampled Systems#
The nonlinear system (1) and its linearized form (3) are given in the continuous-time domain, which is the progressive manifestation of any physical system. However, the output of a system is usually sampled by digital devices in discrete time instances.
Hence, in sampled-data systems the dynamics is described by a continuous-time differential equation, but the output only changes at discrete time instants.
Nonetheless, for numerical considerations the Kalman filter equations are usually given in the discrete-time domain not only at the stage of measure updates (update or correct) but also at the stage of the dynamics propagation (predict).
The discrete-time form of system (3) is given by:
Where:
\(x_k\) is the discretized system state vector
\(F\) is the discretized process dynamics matrix (actually a first order approximation of the state transition matrix \(\Phi\))
\(G\) is the discretized process input matrix
\(u\) is the discretized process input signal
\(\omega_k\) is the process uncertainty with covariance matrix \(Q_k\)
\(y_k\) is the discretized system output vector (the measurement)
\(H\) is the discrete measurement matrix
\(\nu_k\) is the measure noise with covariance matrix \(R_k\)
\(x_0\) is a vector of initial conditions
The noise processes \(\omega_{k}\) and \(\nu_k\) are white, zero-mean, uncorrelated, and have known covariance matrices \(Q_k\) and \(R_k\), respectively:
The discretization of a system is based on the state-transition matrix \(\Phi(t)\). For a matrix \(A\) the state transition matrix \(\Phi(t)\) is given by the matrix exponential \(\Phi = e^{A \cdot t}\) which can be expanded as a power series.
An approximate representation of a continuous-time system by a series expansion up to the first-order is given by:
Where:
\(x_k\) is the discretized system state vector
\(F\) is the discretized process dynamics matrix (actually a first order approximation of the state transition matrix \(\Phi\))
\(G\) is the discretized process input matrix
\(u\) is the discretized process input signal
\(\omega_k\) is the process uncertainty with covariance matrix \(Q_k\)
\(y_k\) is the discretized system output vector (the measurement)
\(H\) is the discrete measurement matrix
\(\nu_k\) is the measure noise with covariance matrix \(R_k\)
\(x_0\) is a vector of initial conditions
\(I\) is the identity matrix
\(dt\) is the sampling time
\(\sim\) is the distribution operator. \(\sim (\mu, \sigma)\) means a normal distribution with mean \(\mu\) and standard deviation \(\sigma\)
\(E(\cdot)\) is the expectation operator
\(\delta(\cdot)\) is the Kronecker delta function (\(\delta(k-j) = 1\) if \(k = j\), and \(\delta_{k-j} = 0\) if \(k \neq j\))
superscript T is the transpose operator
\(Q\) is the process covariance matrix
\(R\) is the measurement covariance matrix
\(A, B, Q_c, R_c\) are the continuous-time system variables of the system state matrix, system input vector, process covariance matrix, and measurement covariance matrix, respectively
Note that the covariance matrices may have been converted from the continuous-time system to discrete-time. However, in most cases, these parameters are determined through experimentation with the system in its final form.
Additionally, measurements are sampled by digital devices at discrete time steps, and the noise properties are typically provided in that form. However, if the process noise applies to a kinematic system where the noise properties are specified in continuous terms, the above approximation can be used or the more exact expression for continuous white noise model \(Q = \int_{0}^{dt} F \cdot Qc \cdot F^T \, dt\)
System Covariance#
Before getting into the Kalman filter itself, it is necessary to consider one more concept, that is the system covariance.
Usually denoted by \(P\), this variable represents the current uncertainty of the estimate.
\(P\) is a matrix that quantifies the estimated accuracy of the state variables, with its diagonal elements indicating the variance of each state variable, and the off-diagonal elements representing the covariances between different state variables.
\(P\) is iteratively refined through the predict and the update steps. Its initial state, \(P_0\), is chosen based on prior knowledge to reflect the confidence in the initial state estimate (\(x_0\)).
Kalman Filter (kalman)#
A simple way to design a Kalman filter is to separate between two steps: predict and update (sometimes called correct). The predict step is used to project the estimate forward in time. The update corrects the prediction by using a new measure.
Predict#
In the prediction step the current estimate is projected forward in time to obtain a predicted estimate using the system model.
The current state estimate, \(x\), is projected into the future using the known system dynamics (4). The uncertainty associated with the predicted state, \(P\), is calculated by projecting the current error covariance forward in time.
Since the predict equations are calculated before a measure is taken (a priori), the new state \(x\) and the new covariance \(P\) are notated by \((-)\) superscript.
Where:
\(x_k^-\) is the estimate of the system state, \(x_k\), before a measurement update.
\(F\) is the discretized process dynamics matrix
\(G\) is the discretized process input matrix
\(u_k\) is the process input signal
\(P_k^-\) is the estimate of the system covariance matrix, \(P_k\), before a measurement update
\(P_{k-1}^+\) is the system covariance matrix estimate, \(P_k\), from previous measurement update
\(Q\) is the process covariance matrix
\(R\) is the measurement covariance matrix
superscript T is the transpose operator
\(x_0\) is the initial state estimation
\(P_0\) is the covariance matrix consisting of errors of the initial estimation
Update#
In the update step (also called correct), the estimate is corrected by using a new measure.
The Kalman gain, \(K\), is computed based on the predicted error covariance and the measurement noise. It determines the optimal weighting between the predicted state and the new measurement.
The predicted state estimate is adjusted using the new measurement, weighted by the Kalman Gain. This update incorporates the latest measurement to refine the state estimate. Then the error covariance is updated to reflect the reduced uncertainty after incorporating the new measurement.
The update equations are calculated after a measure is taken (a posteriori), and the new state \(x\) and the new covariance \(P\) are notated by \((+)\) superscript.
Where:
\(K\) is the Kalman gain
\(P_k^-\) is the estimate of the system covariance matrix, \(P_k\), from the previous prediction
\(H\) is the discrete measurement matrix
\(R\) is the measurement covariance matrix
\(x_k^+\) is the estimate of the system state, \(x_k\), after a measurement update
\(x_k^-\) is the estimate of the system state, \(x_k\), from the previous prediction
\(y\) is the measure
\(I\) is the identity matrix
\(P_k^+\) is the estimate of the system covariance matrix, \(P_k\), after a measurement update
superscript T is the transpose operator
Implementation (C4dynamics)#
kalman
is a discrete linear Kalman filter model.
Following the concept of separating predict
and update, running a Kalman filter is done
by constructing a Kalman filter with parameters as a
state object
and calling the
predict
and update methods.
The Kalman filter in c4dynamics is a class. Thus, the user constructs an object that holds the attributes required to build the estimates. This is crucial to understand because when the user calls the predict or update, the object uses parameters and values from previous calls.
Every filter class in c4dynamics is a subclass of the state class. This means that the filter itself encapsulates the estimated state vector:
>>> from c4dynamics.filters import kalman
>>> import numpy as np
>>> z = np.zeros((2, 2))
>>> kf = kalman(X = {'x1': 0, 'x2': 0}, P0 = z, F = z, H = z, Q = z, R = z)
>>> print(kf)
[ x1 x2 ]
z is an arbitrary matrix used to initialize a filter of two variables (\(x_1, x_2\)).
It also means that a filter object
inherits all the mathematical attributes
(norm, multiplication, etc.)
and data attributes (storage, plotting, etc.)
of a state object
(for further details, see states,
state,
and refer to the examples below)
Example
An altimeter is measuring the altitude of an aircraft. The flight path angle of the aircraft, \(\gamma\) is controlled by a stick which deflects the elevator that in its turn changes the aircaft altitude \(z\):
Where:
\(z\) is the deviation of the aircraft from the required altitude
\(\gamma\) is the flight path angle
\(H_f\) is a constant altitude input required by the pilot
\(\omega_z\) is the uncertainty in the altitude behavior
\(\omega_{\gamma}\) is the uncertainty in the flight path angle behavior
\(u\) is the deflection command
\(y\) is the output measure of z
\(\nu\) is the measure noise
The process uncertainties are given by: \(\omega_z \sim (0, 0.5)[ft], \omega_{\gamma} \sim (0, 0.1)[deg]\).
Let \(H_f\), the required altitude by the pilot to be \(H_f = 1kft\). The initial conditions are: \(z_0 = 1010ft\) (error of \(10ft\)), and \(\gamma_0 = 0\).
The altimeter is sampling in a rate of \(50Hz (dt = 20msec)\) with measure noise of \(\nu \sim (0, 0.5)[ft]\).
A Kalman filter shall reduce the noise and estimate the state variables. But at first it must be verified that the system is observable, otherwise the filter cannot fully estimate the state variables based on the output measurements.
Setup
Import required packages:
>>> from c4dynamics.filters import kalman
>>> from matplotlib import pyplot as plt
>>> from scipy.integrate import odeint
>>> import c4dynamics as c4d
>>> import numpy as np
Define system matrices:
>>> A = np.array([[0, 5], [0, -0.5]])
>>> B = np.array([0, 0.1])
>>> C = np.array([1, 0])
Observability test:
>>> n = A.shape[0]
>>> obsv = C
>>> for i in range(1, n):
... obsv = np.vstack((obsv, C @ np.linalg.matrix_power(A, i)))
>>> rank = np.linalg.matrix_rank(obsv)
>>> print(f'The system is observable (rank = n = {n}).' if rank == n else 'The system is not observable (rank = {rank), n = {n}).')
The system is observable (rank = n = 2).
Some constants and initialization of the scene:
>>> dt, tf = 0.01, 50
>>> tspan = np.arange(0, tf + dt, dt)
>>> Hf = 1000
>>> # reference target
>>> tgt = c4d.state(z = 1010, gamma = 0)
The dynamics is defined by an ODE function to be solved using scipy’s ode integration:
>>> def autopilot(y, t, u = 0, w = np.zeros(2)):
... return A @ y + B * u + w
Ideal system
Let’s start with a simulation of an ideal system. The process has no uncertainties and the radar is clean of measurement errors (isideal flag on):
>>> process_noise = np.zeros((2, 2))
>>> altmtr = c4d.sensors.radar(isideal = True, dt = 2 * dt)
Main loop:
>>> for t in tspan:
... tgt.store(t)
... _, _, Z = altmtr.measure(tgt, t = t, store = True)
... if Z is not None:
... tgt.X = odeint(autopilot, tgt.X, [t, t + dt], args = (Hf - Z, process_noise @ np.random.randn(2)))[-1]
The loop advances the target variables according to the autopilot (accurate) dynamics and the (ideal) measures of the radar.
Plot the time histories of the target altitude (\(z\)) and flight path angle (\(\gamma\)):
>>> fig, ax = plt.subplots(1, 2)
>>> # first axis
>>> ax[0].plot(*tgt.data('z'), 'm', label = 'true')
>>> ax[0].plot(*altmtr.data('range'), '.c', label = 'altimeter')
>>> c4d.plotdefaults(ax[0], 'Altitude', 't', 'ft')
>>> ax[0].legend()
>>> # second axis
>>> ax[1].plot(*tgt.data('gamma', c4d.r2d), 'm')
>>> c4d.plotdefaults(ax[1], 'Path Angle', 't', '')
>>> plt.show()
The ideal altimeter measures the aircraft altitude precisely. Its samples use to control the flight angle that started at an altitude of \(10ft\) above the required altitude (\(Hf = 1000ft\)) and is closed after about \(18s\).
Noisy system
Now, let’s introduce the process uncertainty and measurement noise:
>>> process_noise = np.diag([0.5, 0.1 * c4d.d2r])
>>> measure_noise = 1 # ft
>>> altmtr = c4d.sensors.radar(rng_noise_std = measure_noise, dt = 2 * dt)
Re-running the main loop yields:
Very bad. The errors corrupt the input that uses to control the altitude. The point in which the altitude converges to its steady-state is more than \(10s\) later than the ideal case.
Filtered system
A Kalman filter should find optimized gains to minimize the mean squared error. For the estimated state let’s define a new object, \(kf\), and initialize it with the estimated errors:
>>> z_err = 5
>>> gma_err = 1 * c4d.d2r
>>> tgt = c4d.state(z = 1010, gamma = 0)
>>> kf = kalman(X = {'z': tgt.z + z_err, 'gamma': tgt.gamma + gma_err}
... , P0 = [2 * z_err, 2 * gma_err]
... , R = measure_noise**2 / dt, Q = process_noise**2 * dt
... , F = np.eye(2) + A * dt, G = B * dt, H = C)
The main loop is changed to:
>>> for t in tspan:
... tgt.store(t)
... kf.store(t)
... tgt.X = odeint(autopilot, tgt.X, [t, t + dt], args = (Hf - kf.z, process_noise @ np.random.randn(2)))[-1]
... kf.predict(u = Hf - kf.z)
... _, _, Z = altmtr.measure(tgt, t = t, store = True)
... if Z is not None:
... kf.update(Z)
Plot the state estimates on the true the target altitude (\(z\)) and flight path angle (\(\gamma\)):
>>> fig, ax = plt.subplots(1, 2)
>>> # first axis
>>> ax[0].plot(*tgt.data('z'), 'm', label = 'true')
>>> ax[0].plot(*altmtr.data('range'), '.c', label = 'altimeter')
>>> ax[0].plot(*kf.data('z'), 'y', label = 'kf')
>>> c4d.plotdefaults(ax[0], 'Altitude', 't', 'ft')
>>> ax[0].legend()
>>> # second axis
>>> ax[1].plot(*tgt.data('gamma', c4d.r2d), 'm')
>>> ax[1].plot(*kf.data('gamma', c4d.r2d), 'y')
>>> c4d.plotdefaults(ax[1], 'Path Angle', 't', '')
>>> plt.show()
The filtered altitude (kf.z) is used as input to control the system and generates results almost as good as the ideal case.
Ultimately, the altimeter measuring the aircraft altitude operates through a two-step process: prediction and update. In the prediction step, the filter projects the current state estimate forward using the system model. In the update step, it corrects this prediction with new measurements.
As the Kalman filter implemented as a class, its usage is by creating an instance and then calling its predict and update methods for state estimation.
Extended Kalman Filter (ekf)#
A linear Kalman filter
(kalman)
should be the first choice
when designing a state observer.
However, when a nominal trajectory cannot be found,
the solution is to linearize the system
at each cycle about the current estimated state.
Similarly to the linear Kalman filter, a good approach to design an extended Kalman filter is to separate it to two steps: predict and update (correct).
Since the iterative solution to the algebraic Riccati equation (uses to calculate the optimal covariance matrix \(P\)) involves the matrix representation of the system parameters, the nonlinear equations of the process and / or the measurement must be linearized before executing each stage of the ekf.
Nevertheless, the calculation of the state vector \(x\) both in the predict step (projection in time using the process equations) and in the update step (correction using the measure equations) does not have to use the approximated linear expressions (\(F, H\)) and can use the exact nonlinear equations (\(f, h\)).
Recall the mathematical model of a nonlinear system as given in (1):
Where:
\(f(\cdot)\) is an arbitrary vector-valued function representing the system dynamics
\(x\) is the system state vector
\(u\) is the process input signal
\(\omega\) is the process uncertainty with covariance matrix \(Q\)
\(y\) is the system output vector
\(h(\cdot)\) is an arbitrary vector-valued function representing the system output
\(\nu\) is the measure noise with covariance matrix \(R\)
\(x_0\) is a vector of initial conditions
The noise processes \(\omega(t)\) and \(\nu(t)\) are white, zero-mean, uncorrelated, and have known covariance matrices \(Q\) and \(R\), respectively:
Where:
\(\omega\) is the process uncertainty with covariance matrix \(Q\)
\(\nu\) is the measure noise with covariance matrix \(R\)
\(Q\) is the process covariance matrix
\(R\) is the measurement covariance matrix
\(\sim\) is the distribution operator. \(\sim (\mu, \sigma)\) means a normal distribution with mean \(\mu\) and standard deviation \(\sigma\)
\(E(\cdot)\) is the expectation operator
\(\delta(\cdot)\) is the Dirac delta function (\(\delta(t) = \infty\) if \(t = 0\), and \(\delta(t) = 0\) if \(t \neq 0\))
superscript T is the transpose operator
The linearized term for \(f\) is given by its Jacobian with respect to \(x\):
Note however that the derivatives are taken at the last estimate (as opposed to a nominal trajectory that is used in a global linearization).
The linearized term for \(h\) is given by its Jacobian with respect to \(x\):
A last final step before getting into the filter itself is to discretize these terms:
Where:
\(F\) is the discretized process dynamics matrix (actually a first order approximation of the state transition matrix \(\Phi\))
\(H\) is the discrete measurement matrix
\(I\) is the identity matrix
\(dt\) is the sampling time
\(A, C\) are the continuous-time system dynamics and output matrices
Note that \(Q\) and \(R\) refer to the covariance matrices representing the system noise in its final form, regardless of the time domain.
Now the execution of the predict step and the update step is possible.
Predict#
As mentioned earlier, the advancement of the state vector is possible by using the exact equations. The second in the following equations is an Euler integration to the nonlinear equations.
The progression of the covariance matrix must use the linear terms that were derived earlier. The first equation in the following set is the linearization of the process equations for the covariance calculation (third):
subject to initial conditions:
Where:
\(F\) is the discretized process dynamics matrix
\(I\) is the identity matrix
\(f(\cdot)\) is a vector-valued function representing the system dynamics
\(dt\) is the sampling time
\(x_k^-\) is the estimate of the system state, \(x_k\), before a measurement update.
\(u_k\) is the process input signal
\(P_k^-\) is the estimate of the system covariance matrix, \(P_k\), before a measurement update
\(P_{k-1}^+\) is the system covariance matrix estimate, \(P_k\), from previous measurement update
\(Q\) is the process covariance matrix
superscript T is the transpose operator
\(x_0\) is the initial state estimation
\(P_0\) is the covariance matrix consisting of errors of the initial estimation
Update#
In a similar manner, the measurement equations \(h(x)\) are linearized (\(H\)) before the update to correct the covariance matrix. But the correction of the state vector is possible by using the nonlinear equations themselves (third equation):
Where:
\(H\) is the discrete measurement matrix
\(h(\cdot)\) is a vector-valued function representing the measurement equations
\(x_k^-\) is the estimate of the system state, \(x_k\), from the previous prediction
\(K\) is the Kalman gain
\(P_k^-\) is the estimate of the system covariance matrix, \(P_k\), from the previous prediction
\(R\) is the measurement covariance matrix
\(x_k^+\) is the estimate of the system state, \(x_k\), after a measurement update
\(y\) is the measure
\(I\) is the identity matrix
\(P_k^+\) is the estimate of the system covariance matrix, \(P_k\), after a measurement update
superscript T is the transpose operator
Implementation (C4dynamics)#
We saw that in both the predict and update stages, the state doesn’t have to rely on approximated nonlinear equations but can instead use exact models for the process and the measurement. However, it is sometimes more convenient to use the existing linear for state advancements. C4dyanmics provides an interface for each approach: the predict method can either take \(f(x)\) as an input argument or use the necessary matrix \(F\) to project the state in time. Similarly, the update method can either take \(h(x)\) as an input argument or use the necessary matrix \(H\) to correct \(x\).
Recall a few additional properties of filter implementation in c4dynamics, as described in the linear kalman section:
A. An Extended Kalman filter is a class. The object holds the attributes required to build the estimates, and every method call relies on the results of previous calls.
B. The Extended Kalman filter is a subclass of the state class. The state variables are part of the filter object itself, which inherits all the attributes of a state object.
C. The filter operations
are divided into separate predict and update methods.
ekf.predict
projects the state into
the next time.
ekf.update
calculates the optimized gain and
corrects the state based on the input measurement.
Example
The following example appears in several sources. [ZP] provides a great deal of detail. Additional sources can be found in [SD]. The problem is to estimate the ballistic coefficient of a target in a free fall where a noisy radar is tracking it.
The process equations are:
Where:
\(\rho_0 = 0.0034\)
\(k = 22,000\)
\(g = 32.2 ft/sec^2\)
\(\omega_{\beta} \sim (0, 300)\)
\(\nu_k \sim (0, 500)\)
\(z\) is the target altitude (\(ft\))
\(v_z\) is the target vertical velocity (\(ft/sec\))
\(\beta\) is the target ballistic coefficient (\(lb/ft^2\))
\(y\) is the system measure
Let:
The lineariztion of the process matrix for the predict step:
The measurement is a direct sample of the altitude of the target so these equations are already a linear function of the state.
We now have all we need to run the extended Kalman filter.
Quick setup for an ideal case:
>>> dt, tf = .01, 30
>>> tspan = np.arange(0, tf + dt, dt)
>>> dtsensor = 0.05
>>> rho0, k = 0.0034, 22000
>>> tgt = c4d.state(z = 100000, vz = -6000, beta = 500)
>>> altmtr = c4d.sensors.radar(isideal = True, dt = dt)
Target equations of motion:
>>> def ballistics(y, t):
... return [y[1], rho0 * np.exp(-y[0] / k) * y[1]**2 * c4d.g_fts2 / 2 / y[2] - c4d.g_fts2, 0]
Main loop:
>>> for t in tspan:
... tgt.store(t)
... tgt.X = odeint(ballistics, tgt.X, [t, t + dt])[-1]
... _, _, z = altmtr.measure(tgt, t = t, store = True)
These figures show the time histories of the altitude, velocity, and ballistic coefficient, for a target in a free fall with ideal conditions.
Let’s examine the ekf capability to estimate \(\beta\) at the presence of errors. Errors in initial conditions introduced into each one of the variables: \(z_{0_{err}} = 25, v_{z_{0_{err}}} = -150, \beta_{0_{err}} = 300\). The uncertainty in the ballistic coefficient is given in terms of the spectral density of a continuous system, such that for flight time \(t_f\), the standard deviation of the ballistic coefficient noise is \(\omega_{\beta} = \sqrt{\beta_{err} \cdot t_f}\). The measurement noise is \(\nu = \sqrt{500}\). These use for the noise covariance matrices \(Q, R\) as for the initialization of the state covariance matrix \(P\):
>>> zerr, vzerr, betaerr = 25, -150, 300
>>> nu = np.sqrt(500)
>>> p0 = np.diag([nu**2, vzerr**2, betaerr**2])
>>> R = nu**2 / dt
>>> Q = np.diag([0, 0, betaerr**2 / tf * dt])
>>> H = [1, 0, 0]
>>> tgt = c4d.state(z = 100000, vz = -6000, beta = 500)
>>> # altmeter and ekf construction:
>>> altmtr = c4d.sensors.radar(rng_noise_std = nu, dt = dtsensor)
>>> ekf = c4d.filters.ekf(X = {'z': tgt.z + zerr, 'vz': tgt.vz + vzerr
... , 'beta': tgt.beta + betaerr}
... , P0 = p0, H = H, Q = Q, R = R)
The main loop includes the simulation of the target motion, the linearization and discretization of the process equations, and calling the predict method. Then linearization and discretization of the measurement equations (not relevant here as the measurement is already linear), and calling the update method.
>>> for t in tspan:
... tgt.store(t)
... ekf.store(t)
... # target motion simulation
... tgt.X = odeint(ballistics, tgt.X, [t, t + dt])[-1]
... # process linearization
... rhoexp = rho0 * np.exp(-ekf.z / k) * c4d.g_fts2 * ekf.vz / ekf.beta
... fx = [ekf.vz, rhoexp * ekf.vz / 2 - c4d.g_fts2, 0]
... f2i = rhoexp * np.array([-ekf.vz / 2 / k, 1, -ekf.vz / 2 / ekf.beta])
... # discretization
... F = np.array([[0, 1, 0], f2i, [0, 0, 0]]) * dt + np.eye(3)
... # ekf predict
... ekf.predict(F = F, fx = fx, dt = dt)
... # take a measure
... _, _, Z = altmtr.measure(tgt, t = t, store = True)
... if Z is not None:
... ekf.update(Z)
Though the update requires also the linear process matrix (\(F\)), the predict method stores the introduced F to prove that the update step always comes after calling the predict.
A few steps to consider when designing a Kalman filter:
Spend some time understanding the dynamics. It’s the basis of great filtering.
If the system is nonlinear, identify the nonlinearity; is it in the process? in the measurement? both?
Always prioriorotize linear Kalman. If possible, find a nominal trajectory to linearize the system about.
The major time-consuming activity is researching the balance between the noise matrices Q and R.
-> Plan your time in advance.
Use a framework that provides you with the most flexibility and control.
Make fun!
Low-pass Filter#
A first-order low-pass filter is a fundamental component in signal processing and control systems, designed to allow low-frequency signals to pass while attenuating higher-frequency noise.
This type of filter is represented by a simple differential equation and is commonly used for signal smoothing and noise reduction.
A low-pass filter (LPF) can be described by the differential equation:
Where:
\(y\) is the output signal
\(x\) is the input signal
\(\alpha\) is a shaping parameter that influences the filter’s cutoff frequency
In signal processing, the LPF smooths signals by reducing high-frequency noise. In control systems, it is often used to model a first-order lag.
Frequency-Domain#
In the frequency domain, the transfer function of a first-order low-pass filter is given by:
Where:
\(H(s)\) is the transfer function
\(Y(s)\) and \(X(s)\) are the Laplace transforms of the output and input signals respectively
\(s\) is the complex frequency variable in the Laplace transform, defined as \(s = j \cdot 2 \cdot \pi \cdot f\)
\(\alpha\) is a constant related to the cutoff frequency
Time-Constant#
The cutoff frequency \(f_c\) is the frequency at which the filter attenuates the signal to approximately 70.7% (-3dB) of its maximum value. It is related to the time constant \(\tau\) by:
and equivalently,
In practical applications, the desired cutoff frequency determines \(\tau\), which in turn defines the filter behavior.
Discrete-Time#
In the discrete-time domain, a first-order low-pass filter is represented as:
where \(y_k\) and \(x_k\) are the discrete output and input signals at sample index k, and \(\alpha\) is the filter coefficient derived from the sample rate and cutoff frequency.
Implementation (C4dynamics)#
This filter class can be initialized with a cutoff frequency and sample rate, allowing users to simulate first-order systems.
References
Simon, Dan, ‘Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches’, Hoboken: Wiley, 2006.
Agranovich, Grigory, Lecture Notes on Modern and Digital Control Systems, University of Ariel, 2012-2013.
Zarchan, Paul, ‘Tactical and Strategic Missile Guidance’, American Institute of Aeronautics and Astronautics, 1990.
Meri, Ziv, Extended Lyapunov Analysis and Simulative Investigations in Stability of Proportional Navigation Guidance Systems, MSc. Thesis supervised by prof. Grigory Agranovich, University of Ariel, 2020.