where
is the nonlinear function. In this work, Euler’s numerical integration method with a fixed time step is used for deriving the difference equation. In practice, the values of the noises
and
are not known and
is also set to zero because in the proposed system there is no control input; therefore the state and measurement vectors have to be approximated by
where
is the estimate of the state based on the measurement (a posteriori).
The variables
and
are assumed to be Gaussian with zero mean and are represented by their covariance matrices
and
.
represents a normal distribution:
Two kinds of estimation errors can be defined, the
priori estimation error
and the a
posteriori
estimation error
. They are described by
where
is the a priori state estimate based on knowledge of the process prior to step
and
is the
posteriori state based on the measurement
. The covariances of the estimation errors are defined by
where
is the
priori estimate error covariance and
is the
posteriori estimate error covariance.
The EKF algorithm consists of two stages, time update and measurement update [
24
]. The different steps in the time update stage are
where
is the Jacobian of the system model equations
.
The different steps in the measurement update stage are
where
is the Kalman gain that minimizes the posteriori estimate error covariance
and
is the Jacobian of the measurement function
.
Although, the variables
and
in (
1
) are assumed to be Gaussian with zero mean, the assumption Gaussian is not often practically satisfied. Hence, the matrices
and
cannot be seen as covariance matrices, but as high level tuning parameters, leading to suboptimal solutions. Additionally, the linearization in EKF can lead to poor performance and divergence of the filter for highly nonlinear problems. An improvement to the EKF is some local nonlinear estimators, divided difference filter, unscented Kalman filter (UKF), and so forth. The UKF approximates the probability density resulting from the nonlinear transformation of a random variable instead of approximating the nonlinear functions with a Taylor series expansion. The UKF has a slightly optimal performance compared to the EKF when used in a vehicle navigation state estimation system. However, since the low dynamics of a vehicle make the potential linearization errors of the EKF negligible and the computational time of the UKF is much greater than that of the EKF, the EKF is still chosen in this work to do the vehicle state estimation [
25
].
3.1. Sensor Fusion Strategy
Based on the sensor measurements, as listed in Table
1
, and a vehicle kinematic model, an EKF can be designed to estimate some important states of the Smart. An overview of the sensor configuration for the Smart is shown in Figure
3
. In this configuration, INS measurements are also used to update the vehicle estimated position and heading when DGPS update is not available.
3.2. Extended Kalman Filters Design for the Smart Car
The motion of a vehicle will be referred to as a navigation frame
which is described by a right handed orthogonal axis system (
) on the Earth. A second reference frame (base frame)
fixed in the vehicle is described by axes (
) fixed along the central principal axes of the vehicle. The relation between navigation and base frame can be seen in Figure
4
.
The sensor measurements will be assigned into different frames. DGPS longitudinal, lateral positions, and heading are in the navigation frame. The acceleration and wheel speed measured from INS are in the base frame. To obtain a trajectory in the navigation frame it is necessary to convert the inertial signals from the base frame to the navigation frame, where we assume that the road is flat and then neglect the
and
axes. This can be done by the following matrix (a simplified form of the direct cosine matrix):
3.2.1. Vehicle Four-Wheel Model
In this section, the EKF based on a kinematic four-wheel vehicle model is designed, which is shown in Figure
5
. The assumption is that the test track, on which the Smart is driven, is flat.
In order to reproduce the trajectory followed by the vehicle, kinematic rules for the model have been used to describe the motion of a vehicle. The kinematic vehicle model can be developed as
where
and
are longitudinal and lateral positions in the navigation frame and
indicate the longitudinal and lateral velocities in the base frame. Additionally, for each wheel speed we have
where
are wheel speeds measured from the front left, front right, rear left, and rear right wheels,
is the yaw rate,
are lengths from front and rear axles to the Smart COG, and
are the length of front and rear track. Equations (
12
) and (
13
) are derived from the condition that rear wheels are not steering wheels and their wheel angles are equal to zero.
All model states are described in Table
2
, where the bias sates are modelled as random walks. By using the random walk model, growth of uncertainty of the true value of the bias and the rate at which it varies can be reflected [
26
]. Additionally, the measurements of the EKF are identified in Table
3
.
In navigation frame
In base frame
: LONG position
: LONG acceleration [m/s
2
]
: LAT position
: LAT acceleration [m/s
2
]
: yaw angle
: LONG SPD in COG [m/s]
: yaw rate
: LAT SPD in COG [m/s]
: bias state for LONG ACC
: bias state for LAT ACC
: bias state for GYRO
: bias state, front left ODO
: bias state, front right ODO
: bias state for rear ODO
In the navigation frame
In the base frame
: DGPS LONG
: LONG ACC
: DGPS LAT
: LAT ACC
: DGPS heading
: front left wheel SPD
: GYRO yaw rate
: front right wheel SPD
: rear wheels mean SPD
By using Euler’s method with time step
, the discrete time state equations can be written as
Additionally, the measurement equations can be specified as follows:
3.2.2. System Observability
A dynamic system is said to be observable if it is possible to uniquely reconstruct the state information based on the model of a system given the inputs and outputs of the system. The nonlinear model for the Smart car is
where
= col(
):
.
Although this model is nonlinear, it is linearized each time for the EKF. The observability matrix,
, is calculated for each linearization in the run of the EKF. If
has full rank at each run, then the linearized model is locally observable. In this project, the observability matrix is accumulated over the entire time step from the initial to the final as a global check [
27
]. Since, a state space discrete time model is used in this work, the transient event is not studied and then is neglected from the observability check.
The model is locally observable when the vehicle is driving; when the vehicle is standing still and yaw rate measurement is equal to zero, the estimated longitudinal/lateral COG velocities
/
and bias states for front/rear odometers
,
, and
become unobservable. This is because when the vehicle is standing still no speed information is received from wheel encoders, so nothing can be said about the error in the speed calculation.
4. Fault Detection and Isolation System Design
The generalized observer scheme (GOS) provides that an estimator dedicated to a certain sensor is driven by all outputs except that of the respective sensor. This scheme allows one to detect and isolate only a single fault in any of the sensors, however, with increased robustness with respect to unknown inputs.
A GOS is suitable for detecting a single fault at a time in one of the
sensors of the system. The
observer (
) is driven by all but the
measured variable (i.e.,
). Consequently, each residual from the
observer will be sensitive to all but the
sensor fault.
is not used in the
observer because
is assumed to be corrupted by the fault and therefore does not carry usable information about the system [
3
].
The nonlinear system equations included sensor faults and unknown disturbances are
where
= col(
):
.
is the
-dimensional state vector.
is the
-dimensional vector of measurements. Moreover, the model is subject to the fault signal
as well as to the unknown disturbance signal
. The fault magnitude of
is an arbitrary scalar function of time that is zero when there is no fault.
A GOS can be designed for the sensor FDI, where there are
EKFs, each using all but the
sensor measurement. For each EKF, its residual
can be generated from the measurement update stage as
Because each EKF is excited by all but one sensor output, when a sensor fault occurs in the
sensor, the residual will satisfy the following isolation logic:
where
(
) are isolation thresholds [
28
]. Such a GOS scheme is shown in Figure
6
.
A GOS bank of EKFs is robust to system modeling error and unknown disturbances in a realistic environment. This is because in a GOS only if all
residuals in (
28
) have to misfire, a bad fault decision can be made.
Here, an example is given to illustrate how a GOS can accurately diagnose a single sensor fault in the system, as shown in Figure
6
. In Table
4
, each row represents a fault, where a number of 1 in position
row and
column implies that fault
affects residual
. As stated before, each residual generated from GOS is sensitive to all but one sensor fault and, therefore, for the occurrence of each fault, there exists a unique combination of the residual response, which is so called generalized residual set, as can be noticed in each row in Table
4
. By such a residual table, any single sensor fault
, if it happens, can be uniquely detected and isolated.
4.1. General Outline of the FDI System
The basic scheme of the FDI system is specified in Figure
7
, where fault detection system is to identify a fault that occurred and fault isolation system can determine the location of the fault. The extra fault management system is used to recognize and handle the fault, which contains a state selection system.
4.2. EKFs in the GOS
There are eight sensors installed in Smart car, which are DGPS, longitudinal/lateral accelerometers, gyroscope, and a wheel speed sensor on each vehicle wheel. In order to detect a single fault in these sensors, a GOS bank of EKFs is developed, where there are eight EKFs. In case a sensor fault happens, the output from residual evaluation block (the diagnosis information) will indicate which fault happens and subsequently select the state estimated from the EKF, using all but the faulty sensor measurement, as the fault-free output, which is shown in Figure
8
. It can be seen in the figure that residuals and states generated from each EKF are inputted to the residual evaluation and fault management system, respectively. Hereafter, each EKF (in GOS) for vehicle state estimation will be evaluated, where EKF 0 uses all sensor measurements (nominal EKF).
EKF 1: Driven by All Measurements but DGPS
. This one uses all but DGPS data. In case DGPS fault happens, the estimated states from this EKF will be applied.
EKF 2: Driven by All Measurements but Longitudinal Accelerometer
. All but longitudinal acceleration measurement are used by EKF 2. When longitudinal accelerometer is faulty, EKF 2 will be applied to state estimation. It can be seen in Figure
9
that the estimated longitudinal acceleration is reliable as an analytical redundancy to replace the faulty accelerometer measurement.
EKF 3: Driven by All Measurements but Lateral Accelerometer
. EKF 3 is driven by all but lateral acceleration signal. At the moment a lateral accelerometer fault occurs, states estimated from EKF 3 will be applied.
EKF 4: Driven by All Measurements but Gyroscope
. This EKF uses all but gyroscope measurement. In case gyroscope is faulty, EKF 4 will be used for vehicle state estimation.
EKF 5/6/7/8: Driven by All Measurements but Each Wheel Speed Sensor
. EKF 5 is driven by all but front left wheel speed signal. When this sensor fault happens, the estimated states from EKF 5 are still fault-free. EKFs 6 to 8 use all but other three wheel speed signals, respectively, and can achieve almost the same estimates results as EKF 5. Therefore, the detailed descriptions of these three EKFs are omitted. In case any one of these wheel speed sensors is faulty, the corresponding EKF will be applied to the state estimation.
4.3. Sensor Fault Detection and Detectability
In the GOS, ideally there is a residual generated from each EKF, which is sensitive to all but one sensor fault. To this end, we need to check in each EKF if those sensor faults are detectable with respect to the generated residual. It requires that transfer functions from these sensor faults to the residual are nonzero.
4.3.1. Sensor Faults Description
The FDI system for the Smart is designed to diagnose eight sensor faults, which are listed in Table
5
. The modeling of sensor fault
can be seen in (
26
), where
corresponds to each sensor measurement.
Fault
Description
DGPS fault (in position and heading signals)
Longitudinal accelerometer fault
Lateral accelerometer fault
Gyroscope fault
Front left wheel speed sensor fault
Front right wheel speed sensor fault
Rear left wheel speed sensor fault
Rear right wheel speed sensor fault
4.3.2. Residual Generation
This section discusses several possibilities for residual generation from each EKF. After comparison, the most promising one is selected. At the beginning, a residual notation is made and will be applied throughout this work. Residual
, is the
residual generated from
EKF in the GOS. Because there are eight EKFs in the GOS, we have
. Meanwhile, as seven measurement equations are applied in the EKF 0, seven residuals might be generated from the GOS. To make a clear notation, the order of these seven residuals is fixed as
residual generated from the measurement update: DGPS,
-acceleration,
-acceleration, yaw rate, front left wheel speed, front right wheel speed, and mean rear wheel speed}.
DGPS signals can be blocked by buildings, trees, bridges, and so forth. In this work, the DGPS outage is simply detected by the following logic.
(1)
The number of satellites dropped below 4.
(2)
The value of HDOP is larger than 10, when the positional measurements should be used only to indicate a very rough estimate of the current location.
For INS FDI system design, residual generation in EKF 1 is taken as an example. As using only no DGPS measurements, this EKF can generate residuals from all INS measurement updates. The generated residuals,
to
, are listed in Table
6
, where
is yaw rate,
are the estimated COG velocity,
is the length of the Smart track, and
is the length from the Smart COG to the front axle. It should be advised that although EKF 1 is driven by all seven INS measurements, only six residuals can be generated. The reason is that the mean rear wheel speed, instead of two separate speeds, is used in the measurement update of the EKF 1, which can be seen in (
23
).
Generation
Math formula
ACC
ACC
Gyroscope
Font left ODO
Font right ODO
Rear wheels
Therefore, in each EKF of the GOS, there is a generated residual vector
, which contains all those six residuals but one. In order to construct a GOS, a vector residual or a scalar residual, which is affected by all sensor faults but one, needs to be generated from each EKF.
Scalar residuals are considered in this work. With respect to INS FDI, a scalar residual should be selected among
to
, which is affected by as many sensor faults as possible. Noticed in Table
6
, there are more states involved for the generation of residuals
and
. Thus, either
or
may be applied to the sensors FDI system design. In the following, the detectability of each INS fault with respect to the residual
will be checked.
4.3.3. Theory for Checking the Fault Detectability
When faults occur in the monitored process, the response of the residual vector is
where
is defined as a fault transfer matrix which represents the relation between the residual and faults,
is the
column of
, and
is the
component of
. If
, the
is detectable in the residual
. This is defined as the fault detectability condition of the residual
to
[
28
].
The vehicle model applied in this project is nonlinear and linearized at each time step for EKF. Euler’s numerical integration method with a fixed time step is used for deriving the linearized equation. Therefore, we propose to check the sensor fault local detectability at each time step for a linearized discrete time model. The model is shown as follows:
With this system, a discrete time Kalman filter measurement update equation can be written as
where
is Kalman gain and
is a shift operator in discrete time model. From (
31
) the transfer function from the state vector to the measurement vector can be represented as
Consequently, under normal conditions,
where the subscript
means the normal condition and the subscript
in the following denotes a fault condition. Now, if a measurement is disturbed by a fault vector
,
Then, from (
34
), the state estimation becomes
The normal filter residual can be defined as