6.2.1 Discrete Random Variable

A discrete random variable X has a finite or countable sample space, which contains all the possible outcomes of the random variable X. The probability that the outcome of a discrete random variable X is equal to x is denoted as

$\begin{array}{l}\hfill P(X=x)\end{array}$

or in a shorter notation P(x) = P(X = x). The range of the probability function P(x) is inside the bounded interval between 0 and 1 for every value in the sample space of X, that is, P(x) ∈ [0, 1] ∀ x ∈ X. The sum of the probabilities of all the possible outcomes of the random variable X is equal to 1:

$\begin{array}{l}\hfill \sum _{x\in X}P(x)=1\end{array}$

The probability of two (or more) events occurring together (e.g., random variable X takes value x and random variable Y takes value y) is described by joint probability:

$\begin{array}{l}\hfill P(x,y)=P(X=x,Y=y)\end{array}$

If random variables X and Y are independent, the joint probability (6.2) is simply a product of marginal probabilities:

$\begin{array}{l}\hfill P(x,y)=P(x)P(y)\end{array}$

Conditional probability, denoted by P(x|y), gives the probability of random variable X taking the value x if it is a priori known that the random variable Y has value y. If P(y) > 0, then the conditional probability can be obtained from

$\begin{array}{l}\hfill P(x|y)=\frac{P(x,y)}{P(y)}\end{array}$

In the case of independent random variables X and Y the relation (6.3) becomes trivial:

$\begin{array}{l}\hfill P(x|y)=\frac{P(x,y)}{P(y)}=\frac{P(x)P(y)}{P(y)}=P(x)\end{array}$

One of the fundamental results in probability theory is the theorem of total probability:

$\begin{array}{l}\hfill P(x)=\sum _{y\in Y}P(x,y)\end{array}$

In the case that conditional probability is available, the theorem (6.4) can be given in another form:

$\begin{array}{l}\hfill P(x)=\sum _{y\in Y}P(x|y)P(y)={\mathit{p}}^T(x|Y)\mathit{p}(Y)\end{array}$

In (6.5) the discrete probability distribution was introduced, defined as column vector p(Y ):

$\begin{array}{l}\hfill \mathit{p}(Y)={[P(y_1),P(y_2),\dots ,P(y_N)]}^T\end{array}$

where $N\in \mathbb{N}$ is the number of all the possible states the random variable Y can take. Similarly, the column vector p(x|Y ) is defined as follows:

$\begin{array}{l}\hfill \mathit{p}(x|Y)={[P(x|y_1),P(x|y_2),\dots ,P(x|y_N)]}^T\end{array}$

Discrete probability distributions can be visualized with histograms (Fig. 6.1). In accordance with Eq. (6.1), the height of all the columns in a histogram sums to one. When all the states of the random variable are equally plausible, the random variable can be described with a uniform distribution (Fig. 6.1B).

6.2.2 Continuous Random Variable

The range of a continuous random variable belongs to an interval (either finite or infinite) of real numbers. In the continuous case, it holds P(X = x) = 0, since the continuous random variable X has an infinite sample space. Therefore a probability density function p(x) is introduced that has a bounded range between 0 and 1, that is, p(x) ∈ [0, 1]. The probability that the outcome of a continuous random variable X is less than a is

$\begin{array}{l}\hfill P(X<a)={\int }_{-\infty }^{a}p(x)dx\end{array}$

Examples of a probability density function are shown in Fig. 6.2. Similarly as Relation (6.1) holds for a discrete random variable, the integral of probability density function over the entire sample space of a continuous random variable X is also equal to one:

$\begin{array}{l}\hfill P(-\infty <X<+\infty )={\int }_{-\infty }^{+\infty }p(x)dx=1\end{array}$

Similar relations that hold for a discrete random variable (introduced in Section 6.2.1) can also be extended to a probability density function. Some important relations for discrete and continuous random variables are gathered side-by-side in Table 6.1.

Table 6.1

Selected equations from probability theory for discrete and continuous random variables

Description	Discrete random variable	Continuous random variable
Total probability	${\sum }_{x\in X}P(x)=1$	${\int }_{-\infty }^{+\infty }p(x)dx=1$
Theorem of total	$P(x)={\sum }_{y\in Y}P(x,y)$	$p(x)={\int }_{-\infty }^{+\infty }p(x,y)dy$
probability	$P(x)={\sum }_{y\in Y}P(x|y)P(y)$	$p(x)={\int }_{-\infty }^{+\infty }p(x|y)p(y)dy$
Mean	${\mu }_X={\sum }_{x\in X}xP(x)$	${\mu }_X={\int }_{-\infty }^{+\infty }xp(x)dx$
Variance	${\sigma }_X^2={\sum }_{x\in X}{(x-{\mu }_X)}^{2}P(x)$	${\sigma }_X^2={\int }_{-\infty }^{+\infty }{(x-{\mu }_X)}^{2}p(x)dx$

The probability distributions of the random variable are seldom described by various statistics. The mean value of a continuous random variable X is defined as a mathematical expectation:

$\begin{array}{l}\hfill {\mu }_X=\text{E}\left\{X\right\}={\int }_{-\infty }^{+\infty }xp(x)dx\end{array}$

One of the basic parameters that describe the shape of distribution is variance that is defined as follows:

$\begin{array}{l}\hfill {\sigma }_X^2=\text{var}\left\{X\right\}=\text{E}\left\{{(X-\text{E}\left\{X\right\})}^2\right\}={\int }_{-\infty }^{+\infty }{(x-{\mu }_X)}^{2}p(x)dx\end{array}$

These properties can also be defined for discrete random variables, and they are given in Table 6.1.

The mean value μ and the variance σ² are the only two parameters that are required to uniquely describe one of the most important probability density functions, a normal distribution (Fig. 6.3), which is presented with a Gaussian function:

$\begin{array}{l}\hfill p(x)=\frac{1}{\sqrt{2\pi {\sigma }^2}}{e}^{-\frac{1}{2}\frac{{(x-\mu )}^2}{{\sigma }^2}}\end{array}$

In a multidimensional case, when a random variable is a vector x, the normal distribution takes the following form:

$\begin{array}{l}\hfill p(\mathit{x})=det{\left(2\pi \mathbf{\Sigma }\right)}^{-\frac{1}{2}}{e}^{-\frac{1}{2}{(\mathit{x}-\mathit{\mu })}^T{\mathbf{\Sigma }}^{-1}(\mathit{x}-\mathit{\mu })}\end{array}$

where Σ is the covariance matrix. An example of a 2D Gaussian function is shown in Fig. 6.4. Covariance is a symmetric matrix where the element in the row i and column j is a covariance $\text{cov}\left\{X_i,X_j\right\}$ between the random variables X_i and X_j.

The covariance $\text{cov}\left\{X,Y\right\}$ is a measure of the linear relationship between random variables X and Y:

$\begin{array}{l}\hfill \begin{array}{cc}{\sigma }_{XY}^2=\text{cov}\left\{X,Y\right\}& =\text{E}\left\{(X-{\mu }_X)(Y-{\mu }_Y)\right\}\\ & ={\int }_{-\infty }^{+\infty }{\int }_{-\infty }^{+\infty }(X-{\mu }_X)(Y-{\mu }_Y)p(x,y)dxdy\end{array}\end{array}$

where p(x, y) is the joint probability density function of X and Y. The relation (6.9) can be simplified to

$\begin{array}{l}\hfill {\sigma }_{XY}^2=\text{E}\left\{XY\right\}-{\mu }_X{\mu }_Y\end{array}$

If random variables X and Y are independent, then $\text{E}\left\{XY\right\}=\text{E}\left\{X\right\}\text{E}\left\{Y\right\}$ and the covariance (6.10) is zero: ${\sigma }_{XY}^2=0$. However, null covariance does not imply that random variables are independent. Covariance of two identical random variables is the following variance: $\text{cov}\left\{X,X\right\}=\text{var}\left\{X\right\}={\sigma }^2(X)$.

6.2.3 Bayes’ Rule

Bayes’ rule is one of the fundamental pillars in probabilistic robotics. The definition of Bayes’ rule is

$\begin{array}{l}\hfill p(x|y)=\frac{p(y|x)p(x)}{p(y)}\end{array}$

for a continuous space and

$\begin{array}{l}\hfill P(x|y)=\frac{P(y|x)P(x)}{P(y)}\end{array}$

for a discrete space. Bayes’ rule normally allows us to calculate the probability that is hard to determine, based on the probabilities that are presumably easier to obtain.

Example 6.3

A mobile robot is equipped with a dirt detection sensor that can detect the cleanliness of the floor underneath the mobile robot (Fig. 6.5). Based on the sensor readings we would like to determine whether the floor below the mobile robot is clean or not; therefore, the discrete random state has two possible values. The probability that the floor is clean is 40%. The following statements can be made about the dirt detection sensor: if the floor is clean, the sensor correctly determines the state of the floor as clean in 80%; and if the floor is dirty, the sensor correctly determines the state of the floor as dirty in 90%. The probability of incorrect measurement is small: if the floor is clean, the sensor will incorrectly determine the state of the floor in 1 out of 5 cases, but if the floor is dirty the sensor performance is even better since it makes an incorrect measurement only in 1 out of 10 cases. What is the probability that the floor is clean if the sensor detects the floor to be clean?

Solution

Let us denote the state of the floor with discrete random variable X ∈{clean, dirty} and the measurement of the sensor with random variable Z ∈{clean, dirty}. The probability that the floor is clean can therefore be written as P(X = clean) = 0.4, and the sensor measurement model can be mathematically represented as follows:

$\begin{array}{l}\hfill \begin{array}{cc}P(Z=clean|X=clean)& =0.8\\ P(Z=dirty|X=dirty)& =0.9\end{array}\end{array}$

Let us introduce a shorter notation:

$\begin{array}{l}\hfill \begin{array}{cc}P(x)& =P(X=clean)=0.4\\ P(\stackrel{-}{x})& =P(X=dirty)=1-P(x)=0.6\\ P(z|x)& =P(Z=clean|X=clean)=0.8\\ P(\stackrel{-}{z}|x)& =P(Z=dirty|X=clean)=1-P(z|x)=0.2\\ P(\stackrel{-}{z}|\stackrel{-}{x})& =P(Z=dirty|X=dirty)=0.9\\ P(z|\stackrel{-}{x})& =P(Z=clean|X=dirty)=1-P(\stackrel{-}{z}|\stackrel{-}{x})=0.1\end{array}\end{array}$

We would like to determine P(x|z) that can be calculated from Bayes’ rule (6.12):

$\begin{array}{l}\hfill P(x|z)=\frac{P(z|x)P(x)}{P(z)}\end{array}$

Using the total probability theorem, the probability that the sensor detects floor as clean can be calculated as follows:

$\begin{array}{l}\hfill \begin{array}{cc}P(z)& =P(z|x)P(x)+P(z|\stackrel{-}{x})P(\stackrel{-}{x})\\ & =0.8\cdot 0.4+0.1\cdot 0.6\\ & =0.38\end{array}\end{array}$

The probability that we are looking for is then

$\begin{array}{l}\hfill P(x|z)=\frac{0.8\cdot 0.4}{0.38}=0.8421\end{array}$

The obtained probability of a clean floor in the case the sensors detect the floor as clean is high. However, there is a more than 15% chance that the floor is actually dirty. Therefore, the mobile system can incorrectly assume that no cleaning is required; and if the mobile robot does not take any cleaning actions, the floor remains dirty.

6.3.1 Disturbances and Noise

All the system dynamics that are not taken into account in the model of the system, all unmeasurable signals, and modeling errors can be thought of as system disturbances. Under linearity assumption all the disturbances can be presented in a single term n(t) that is added to the true signal y₀(t):

$\begin{array}{l}\hfill y(t)=y_0(t)+n(t)\end{array}$

Disturbances can be classified into several classes: high-frequency, quasi-stationary stochastic signal (e.g., measurement noise), low-frequency, nonstationary signal (e.g., drift), periodic signal, or some other type of signal (e.g., spikes, data outliers). One of the most important stochastic signals is white noise.

Frequency spectrum and signal distribution are the most important properties that describe a signal. Signal distribution gives the probability that amplitude takes a particular value. Two of the most common signal distributions are uniform distribution and Gaussian (normal) distribution (see Section 6.2). The frequency spectrum of a signal represents the interdependence of a signal in every time moment, which is related to distribution of frequency components composing the signal. In the case of white noise, the distribution of all frequency components is uniform. Therefore the signal value in every time step is independent of the previous signal’s values.

6.3.2 Estimate Convergence and Bias

As already mentioned, the state estimation provides the estimates of internal states based on the measured input-output signals, the relations among the variables (model of the system), and some statistical properties of signals, for example, variances, and other information about the system. All these pieces of information should be somehow fused in order to obtain accurate and precise estimation of internal states. Unfortunately, the measurements, the model, and the a priori known properties of the signals are inherently uncertain due to noises, disturbances, parasitic dynamics, wrong assumptions about the system model, and other sources of error. As a result, the state estimates are in general different from the actual states.

All the above-mentioned problems cause a certain level of uncertainty in signals. Mathematically, the problem can be coped within a stochastic environment where signals are treated as random variables. The signals are then represented by their probability density functions or sometimes simplified by the mean value and the variance. An important issue in a stochastic environment is the quality of a certain state estimate. Especially, the convergence of a state estimate toward the true value has to be analyzed. In particular two important questions should be asked:

1. Is the mathematical expectation of the estimate the same as the true value? If so, the estimate is bias-free. The estimate is consistent if it improves with time (larger observation interval) and it converges to the true value as the interval of observation becomes infinite.

2. Does the variance of the estimation error converge to zero as the observation time approaches infinity? If so and the estimate is consistent, the estimate is consistent in the mean square. This means that with growing observation time the accuracy and the precision of the estimates become excellent (all the estimates are in the vicinity of the true value).

The answers to the above questions are relatively easy to obtain when the assumptions about the system are simple (perfect model of the system, Gaussian noises, etc.). When a more demanding problem is treated, the answers become extremely difficult and analytical solutions are no longer possible. However, what needs to be remembered is that state estimators provide satisfactory results if some important assumptions are not violated. This is why it is extremely important to read the fine print when using a certain state estimation algorithm. Otherwise, these algorithms can provide the estimations that are far from the actual states. And the problem is that we are usually not even aware of this.

6.3.3 Observability

The states that need to be estimated are normally hidden, since information about the states is usually not directly accessible. The states can be estimated based on measurement of the systems’ outputs that are directly or indirectly dependent on system states. Before the estimation algorithm is implemented the following question should be answered: Can the states be estimated uniquely in a finite time based on observation of the system outputs? The answer to this question is related to the observability of the system. If the system is observable, the states can be estimated based on observing the system outputs. The system may only be partially observable; if this is the case, only a subset of system states can be estimated, and the remaining system states may not be estimated. If the system is fully observable, all the states are output connected.

Before we proceed to the definition of observability, the concept of indistinguishable states should be introduced. Consider a general nonlinear system in the following form ($\mathit{x}\in {\mathbb{R}}^n$, $\mathit{u}\in {\mathbb{R}}^m$, and $\mathit{y}\in {\mathbb{R}}^l$):

$\begin{array}{l}\hfill \begin{array}{cc}\stackrel{.}{\mathit{x}}(t)& =\mathit{f}(\mathit{x}(t),\mathit{u}(t))\\ \mathit{y}(t)& =\mathit{h}(\mathit{x}(t))\end{array}\end{array}$

Two states x₀ and x₁ are indistinguishable if for every admissible input u(t) on a finite time interval t ∈ [t₀, t₁], identical outputs are obtained [2]:

$\begin{array}{l}\hfill \mathit{y}(t,{\mathit{x}}_0)\equiv \mathit{y}(t,{\mathit{x}}_1)\forall t\in [t_0,t_1]\end{array}$

A set of all indistinguishable states from the state x₀ is denoted as $\mathcal{I}({\mathit{x}}_0)$. The definitions of observability can now be given as follows. The system is observable at x₀ if a set of indistinguishable states $\mathcal{I}({\mathit{x}}_0)$ contains only the state x₀, that is, $\mathcal{I}({\mathit{x}}_0)=\{{\mathit{x}}_0\}$. The system is observable if a set of indistinguishable states $\mathcal{I}(\mathit{x})$ contains only the state x for every state x in the domain of definition, that is, $\mathcal{I}(\mathit{x})=\{\mathit{x}\}\forall \mathit{x}$. Notice that observability does not imply that estimation of x from the observed output may be possible for every input u(t), t ∈ [t₀, t₁]. Also note that a long time of observation may be required to distinguish between the states. Various forms of observability have been defined [2]: local observability (stronger concept of observability), weak observability (weakened concept of observability), and local weak observability. In the case of autonomous linear systems these various forms of observability are all equivalent.

Checking the observability of a general nonlinear systems (6.14) requires advanced mathematical analysis. The local weak observability of the system can be checked with a simple algebraic test. For this purpose a Lie derivative operator is introduced as the time derivative of h along the system trajectory x:

$\begin{array}{l}\hfill L_{\mathit{f}}[\mathit{h}(\mathit{x})]=\frac{\partial \mathit{h}(\mathit{x})}{\partial \mathit{x}}\mathit{f}(\mathit{x})\end{array}$

Consider the system (6.14) is autonomous (u(t) = 0 for every t). To check the local weak observability the system output y needs to be differentiated several times (until the rank of the matrix Q, which is defined later on, increases):

$\begin{array}{l}\hfill \begin{array}{cc}\mathit{y}& =\mathit{h}(\mathit{x})=L_{\mathit{f}}^0[\mathit{h}(\mathit{x})]\\ \stackrel{.}{\mathit{y}}& =\frac{\partial \mathit{h}(\mathit{x})}{\partial \mathit{x}}\frac{d\mathit{x}}{dt}=\frac{\partial \mathit{h}(\mathit{x})}{\partial \mathit{x}}\mathit{f}(\mathit{x})=L_{\mathit{f}}[\mathit{h}(\mathit{x})]=L_{\mathit{f}}^1[\mathit{h}(\mathit{x})]\\ \ddot{\mathit{y}}& =\frac{\partial }{\partial \mathit{x}}\left(\frac{\partial \mathit{h}(\mathit{x})}{\partial \mathit{x}}\mathit{f}(\mathit{x})\right)\mathit{f}(\mathit{x})=L_{\mathit{f}}[L_{\mathit{f}}[\mathit{h}(\mathit{x})]]=L_{\mathit{f}}^2[\mathit{h}(\mathit{x})]\\ & ⋮\\ \frac{d^i\mathit{y}}{d{t}^i}& =L_{\mathit{f}}^i[\mathit{h}(\mathit{x})]\end{array}\end{array}$

The time derivatives of the system output (6.17) can be arranged into a matrix L(x):

$\begin{array}{l}\hfill \mathit{L}(\mathit{x})=\left[\begin{array}{c}\hfill L_{\mathit{f}}^0[\mathit{h}(\mathit{x})]\hfill \\ \hfill L_{\mathit{f}}^1[\mathit{h}(\mathit{x})]\hfill \\ \hfill L_{\mathit{f}}^2[\mathit{h}(\mathit{x})]\hfill \\ \hfill ⋮\hfill \\ \hfill L_{\mathit{f}}^i[\mathit{h}(\mathit{x})]\hfill \end{array}\right]\end{array}$

The rank of matrix $\mathit{Q}({\mathit{x}}_0)=\frac{\partial }{\partial \mathit{x}}\mathit{L}(\mathit{x}){|}_{{\mathit{x}}_0}$ determines the local weak observability of the system at x₀. If the rank of the matrix Q(x₀) is equal to the number of states, that is, rank(Q(x₀)) = n, the system is said to satisfy the observability rank condition at x₀, and this is a sufficient, but not necessary, condition for local weak observability of the system at x₀. If the observability rank condition is satisfied for every x from the domain of definition, the system is locally and weakly observable. A more involved study on the subject of observability can be found in, for example, [2–4].

The observability rank condition simplifies for time-invariant linear systems. For a system with n states in the form $\stackrel{.}{\mathit{x}}(t)=\mathit{A}\mathit{x}(t)+\mathit{B}\mathit{u}(t)$, y(t) = Cx(t) the Lie derivatives in Eq. (6.18) are

$\begin{array}{l}\hfill \begin{array}{cc}{L}_{\mathit{f}}^0[\mathit{h}(\mathit{x})]& =\mathit{C}\mathit{x}(t)\\ L_{\mathit{f}}^1[\mathit{h}(\mathit{x})]& =\mathit{C}(\mathit{A}\mathit{x}(t)+\mathit{B}\mathit{u}(t))\\ L_{\mathit{f}}^2[\mathit{h}(\mathit{x})]& =\mathit{C}\mathit{A}(\mathit{A}\mathit{x}(t)+\mathit{B}\mathit{u}(t))\\ & ⋮\end{array}\end{array}$

Partial differentiation of Lie derivatives (Eq. 6.19) leads to the Kalman observability matrix Q:

$\begin{array}{l}\hfill {\mathit{Q}}^T=\left[\begin{array}{cccc}\hfill {\mathit{C}}^T\hfill & \hfill {\mathit{A}}^T{\mathit{C}}^T\hfill & \hfill \cdots \hfill & \hfill {({\mathit{A}}^T)}^{n-1}{\mathit{C}}^T\hfill \end{array}\right]\end{array}$

The system is observable if the rank of the observability matrix has n independent rows, that is, the rank of the observability matrix equals the number of states:

$\begin{array}{l}\hfill rank(\mathit{Q})=n\end{array}$

6.4 Bayesian Filter

6.4.1 Markov Chains

Let us focus on systems for which a proposition about a complete state can be assumed. This means that all the information about the system in any given time can be determined from the system states. The system can be described based on the current states, which is a property of the Markov process. In Fig. 6.6, a hidden Markov process (chain) is shown where the states are not accessible directly and can only be estimated from measurements that are stochastically dependent on the current values of the states. Under these assumptions (Markov process), the current state of the system is dependent only on the previous state and not the entire history of the states:

$\begin{array}{l}\hfill p(x_k|x_0,\dots ,x_{k-1})=p(x_k|x_{k-1})\end{array}$

Similarly, the measurement is also assumed to be independent of the entire history of system states if only the current state is known:

$\begin{array}{l}\hfill p(z_k|x_0,\dots ,x_k)=p(z_k|x_k)\end{array}$

The structure of a hidden Markov process where the current state x_k is dependent not only on the previous state x_k−1 but also on the system input u_k−1 is shown in Fig. 6.7. The input u_k−1 is the most recent external action that influences the internal states from time step k − 1 to the current time step k.

6.4.2 State Estimation From Observations

A Bayesian filter is the most general form of an algorithm for calculation of probability distribution. A Bayesian filter is a powerful statistical tool that can be used for location (system state) estimation in the presence of system and measurement uncertainties [5].

The probability distribution of the state after measurement (p(x|z)) can be estimated if the statistical model of the sensor p(z|x) (probability distribution of the measurement given a known state) and the probability distribution of the measurement p(z) are known. This was shown in Example 6.3 for a discrete random variable.

Let us now take a look at the case of estimating the state x when multiple measurements z are obtained in a sequence. We would like to estimate p(x_k|z₁, …, z_k), that is, the probability distribution of the state x in time step k while considering the sequence of all the measurements until the current time step. The Bayesian formula can be written in a recursive form:

$\begin{array}{l}\hfill p(x_k|z_1,\dots ,z_k)=\frac{p(z_k|x_k,z_1,\dots ,z_{k-1})p(x_k|z_1,\dots ,z_{k-1})}{p(z_k|z_1,\dots ,z_{k-1})}\end{array}$

that can be rewritten into a shorter form:

$\begin{array}{l}\hfill p(x_k|z_{1:k})=\frac{p(z_k|x_k,z_{1:k-1})p(x_k|z_{1:k-1})}{p(z_k|z_{1:k-1})}\end{array}$

The meanings of the terms in Eq. (6.22) are as follows:

• p(x_k|z_1:k) is the estimated state probability distribution in time step k, updated with measurement data.

• p(z_k|x_k, z_1:k−1) is the measurement probability distribution in time step k if the current state x_k and previous measurements up to time k − 1 are known.

• p(x_k|z_1:k−1) is the predicted state probability distribution based on previous measurements.

• p(z_k|z_1:k−1) is the measurement probability (confidence in measurement) in time step k.

Normally it holds that the current measurement z_k in Eq. (6.22) is independent on the previous measurements z_1:k−1 (complete state assumption, Markov process) if the system state x_k is known:

$\begin{array}{l}\hfill p(z_k|x_k,z_{1:k-1})=p(z_k|x_k)\end{array}$

Therefore, Eq. (6.22) simplifies into the following:

$\begin{array}{l}\hfill p(x_k|z_{1:k})=\frac{p(z_k|x_k)p(x_k|z_{1:k-1})}{p(z_k|z_{1:k-1})}\end{array}$

The derivation of Eq. (6.23) is given in Example 6.4.

The recursive formula (6.23), which updates the state based on previous measurements, contains a prediction term p(x_k|z_1:k−1). The state estimation can be separated into two steps: a prediction step and a correction step.

The Most Probable State Estimate

How can the estimated probability density distribution p(x_k|z_1:k) be used in calculation of the most probable state x_k? The best and most probable state estimate (mathematical expectation) $\text{E}\left\{x_k\right\}$ is defined as the value that minimizes the average square error of the measurement:

$\begin{array}{l}\hfill \text{E}\left\{x_k\right\}=\int x_{k}p(x_k|z_{1:k})d{x}_k\end{array}$

Moreover, the value of $x_{k_{max}}$, which maximizes the posterior probability p(x_k|z_1:k), can also be estimated:

$\begin{array}{l}\hfill x_{k_{max}}=\underset{x_k}{max}p(x_k|z_{1:k})\end{array}$

Example 6.5

Consider Example 6.3 again and determine the probability that the floor is clean if the sensor in time step k = 2 again detects the floor to be clean.

Solution

In time step k = 1 of Example 6.3 the following situation was considered: the floor was clean and the sensor also detected the floor to be clean (z₁ = clean). The following conditional probability was calculated:

$\begin{array}{l}\hfill P(x_1|z_1)=\frac{0.8\cdot 0.4}{0.38}=0.8421\end{array}$

In the next time step k = 2 the sensor returns a reading z₂ = clean with probability P(z₂|x₂) = 0.8 of a correct sensor reading and with probability $P(z_2|{\stackrel{-}{x}}_2)=0.1$ of an incorrect sensor reading (the sensor characteristic was assumed to be time invariant).

Let us first evaluate the predicted probability P(x₂|z₁), that is, the floor is dirty based on the previous measurement. Considering Eq. (6.24), where the operation of integration is substituted for the operation of summation, yields

$\begin{array}{l}\hfill P(x_k|z_{1:k-1})=\sum _{x_{k-1}\in X}P(x_k|x_{k-1})P(x_{k-1}|z_{1:k-1})\end{array}$

For now assume that the mobile system only detects the state of the floor, but it cannot influence the state of the floor (i.e., the mobile system does not perform any floor cleaning and also cannot make the floor dirty). Therefore, the state transition probability is simply P(x₂|x₁) = 1 and $P(x_2|{\stackrel{-}{x}}_1)=0$. Hence,

$\begin{array}{l}\hfill \begin{array}{cc}P(x_2|z_{1:1})& =P(x_2|x_1)P(x_1|z_1)+P(x_2|{\stackrel{-}{x}}_1)P({\stackrel{-}{x}}_1|z_1)\\ & =1\cdot 0.8421+0\cdot 0.1579\\ & =0.8421\end{array}\end{array}$

which is a logical result since if the measurement z₂ is not considered, no new information about the system has been obtained. Therefore, the state of the floor has the same probability in the time step k = 2 as it was the state probability in the previous time step k = 1.

The measurement can be fused with the current estimate in the correction step, using relation (6.25):

$\begin{array}{l}\hfill \begin{array}{cc}P(x_2|z_{1:2})& =\frac{P(z_2|x_2)P(x_2|z_{1:1})}{P(z_2|z_{1:1})}\\ & =\frac{0.8\cdot 0.8421}{P(z_2|z_{1:1})}\end{array}\end{array}$

where the value of the normalization factor needs to be calculated. The missing factor is the probability of a clean floor in time step k = 2, and it can be determined with the summation of all the possible state combinations that result in the current measurement z_k, considering the outcomes of the previous measurements z_1:k−1:

$\begin{array}{l}\hfill P(z_k|z_{1:k-1})=\sum _{x_k\in X}P(z_k|x_k)P(x_k|z_{1:k-1})\end{array}$

In our case,

$\begin{array}{l}\hfill \begin{array}{cc}P(z_2|z_1)& =P(z_2|x_2)P(x_2|z_1)+P(z_2|{\stackrel{-}{x}}_2)P({\stackrel{-}{x}}_2|z_1)\\ & =0.8\cdot 0.8421+0.1\cdot 0.1579\\ & =0.6895\end{array}\end{array}$

The final result, the probability that the floor is clean if the sensor has detected the floor to be clean twice in a row, is

$\begin{array}{l}\hfill P(x_2|z_{1:2})=0.9771\end{array}$

Example 6.6

Again consider Example 6.3 and determine how the state probability changes if the floor is clean and the sensor makes three measurements z_1:3 = (clean, clean, dirty).

Solution

The first two conditional probabilities were already calculated in Examples 6.3 and 6.5, the third is

$\begin{array}{l}\hfill \begin{array}{cc}P(x_3|z_{1:3})& =\frac{P(z_3|x_3)P(x_3|z_{1:2})}{P(z_3|z_{1:2})}\\ & =\frac{0.2\cdot 0.9771}{0.2\cdot 0.9771+0.9\cdot (1-0.9771)}\\ & =0.9046\end{array}\end{array}$

where P(Z = dirty|X = clean) = 1 − P(Z = clean|X = clean). The state probabilities in three consequential state measurements taken at time steps i = 1, 2, 3 are therefore

$\begin{array}{l}\hfill P(x_k|z_{1:i})=(0.8421,0.9771,0.9046)\end{array}$

The posterior state probability distribution for time steps k = 1, 2, 3 is also shown in Fig. 6.8. Implementation of the solution in Matlab is shown in Listing 6.2.

Listing 6.2

Implementation of the solution of Example 6.6

1 %  Probabilities of clean and dirtyf l o o r

2 P_Xc= 0.4   %  P (X = clean)

3 P_Xd=  1− P_Xc %  P (X = dirty)

4 %  Conditionalprobabilities  of the dirt sensor measurements

5 P_ZcXc= 0.8   %  P (Z = clean|X = clean)

6 P_ZdXc=  1− P_ZcXc %  P (Z = dirty|X = clean)

7 P_ZdXd= 0.9   %  P (Z = dirty|X = dirty)

8 P_ZcXd=  1− P_ZdXd %  P (Z = clean|X = dirty)

9

10 disp( ’Time step k =  1: Z = clean ’)

11 %  Measurement probability in the case cleanf l o o r i s  detected

12 P_Zc_k1=  P_ZcXc*P_Xc+  P_ZcXd*P_Xd

13 %  Probability of cleanf l o o r  after the measurementi s  made (Bayes’ rule)

14 P_XcZc_k1=  P_ZcXc*P_Xc/P_Zc_k1

15 P_XdZc_k1=  1− P_XcZc_k1 ;

16

17 disp( ’Time step k =  2: Z = clean ’)

18 %  Measurement probability in the case cleanf l o o r i s  detected

19 P_Zc_k2=  P_ZcXc*P_XcZc_k1+  P_ZcXd*P_XdZc_k1

20 %  Probability of cleanf l o o r  after the measurementi s  made (Bayes’ rule)

21 P_XcZc_k2=  P_ZcXc*P_XcZc_k1/P_Zc_k2

22 P_XdZc_k2=  1− P_XcZc_k2 ;

23

24 disp( ’Time step k =  3: Z = dirty ’)

25 %  Measurement probability in the case dirtyf l o o r i s  detected

26 P_Zd_k3=  P_ZdXc*P_XcZc_k2+  P_ZdXd*P_XdZc_k2

27 %  Probability of cleanf l o o r  after the measurementi s  made (Bayes’ rule)

28 P_XcZd_k3=  P_ZdXc*P_XcZc_k2/P_Zd_k3

29 P_XdZd_k3=  1− P_XcZd_k3 ;

30 P_Xc =

31     0.4000

32 P_Xd =

33     0.6000

P_ZcXc =

     0.8000

P_ZdXc =

     0.2000

P_ZdXd =

     0.9000

P_ZcXd =

     0.1000

Time step k = 1: Z = clean

P_Zc_k1 =

     0.3800

P_XcZc_k1 =

     0.8421

Time step k = 2: Z = clean

P_Zc_k2 =

     0.6895

P_XcZc_k2 =

     0.9771

Time step k = 3: Z = dirty

P_Zd_k3 =

     0.2160

P_XcZd_k3 =

     0.9046

6.4.3 State Estimation From Observations and Actions

In Section 6.4.2 the state of the system was estimated based only on observing the environment. Normally the actions of the mobile system can have an effect on the environment; therefore, the system states can change due the mobile system actions (e.g., the mobile system moves, performs cleaning actions, etc.). Every action that the mobile system makes has an inherent uncertainty. Therefore, the outcome of the action is not deterministic, but it has an inherent uncertainty (probability). The probability p(x_k|x_k−1, u_k−1) describes the probability of transition from the previous state into the next state given a known action. The action u_k−1 takes place in time step k − 1 and is active until time step k. Therefore, this action is also known as the most recent or current action. In a general case, the actions that take place in the environment increase the level of uncertainty about our knowledge of the environment, and the measurements made in the environment normally decrease the level of uncertainty.

Let us take a look at what effects the actions and measurements have on our knowledge about the states. We would like to determine the probability p(x_k|z_1:k, u_0:k−1), where z indicates the measurements and u indicates the actions.

As in Section 6.4.2, the Bayesian theorem can be written as follows:

$\begin{array}{l}\hfill p(x_k|z_{1:k},u_{0:k-1})=\frac{p(z_k|x_k,z_{1:k-1},u_{0:k-1})p(x_k|z_{1:k-1},u_{0:k-1})}{p(z_k|z_{1:k-1},u_{0:k-1})}\end{array}$

where the following applies:

• p(x_k|z_1:k, u_0:k−1) is the estimated state probability distribution in time step k, updated with measurement data and known actions.

• p(z_k|x_k, z_1:k−1, u_0:k−1) is the measurement probability distribution in time step k, if the current state x_k, previous actions and measurements up to time step k − 1 are known.

• p(x_k|z_1:k−1, u_0:k−1) is the predicted state probability distribution based on previous measurements and actions made up to time step k.

• p(z_k|z_1:k−1, u_0:k−1) is the measurement probability (confidence in measurement) in time step k.

Note that action indices are in the range from 0 to k − 1, since the actions in the previous time steps influence the behavior of the system states in step k.

Furthermore, the current measurement z_k in Eq. (6.26) can be described only with system state x_k, since the previous measurements and actions do not give any additional information about the system (complete state assumption, Markov process):

$\begin{array}{l}\hfill p(z_k|x_k,z_{1:k-1},u_{0:k-1})=p(z_k|x_k)\end{array}$

Hence, the relation (6.26) can be simplified:

$\begin{array}{l}\hfill p(x_k|z_{1:k},u_{0:k-1})=\frac{p(z_k|x_k)p(x_k|z_{1:k-1},u_{0:k-1})}{p(z_k|z_{1:k-1},u_{0:k-1})}\end{array}$

The recursive rule (6.27), update of the state-probability estimate based on the previous measurements and actions, also includes prediction p(x_k|z_1:k−1, u_0:k−1), where the probability distribution for state estimate is made based on previous measurements z_1:k−1 and all the actions u_0:k−1. Therefore, the state estimation procedure can be split into a prediction step and a correction step. In the prediction step the latest measurement is not known yet, but the most recent action is known. Therefore, the state probability distribution can be estimated based on the system model. When the new measurement is available, the correction step can be made.

General Algorithm for a Bayesian Filter

The general form of the Bayesian algorithm is given as a pseudo code in Algorithm 3. The conditional probability of the correction step p(x_k|z_1:k, u_0:k−1), which gives the state probability distribution estimate based on known actions and measurements, is also known as belief, and it is normally denoted as

$\begin{array}{l}\hfill bel(x_k)=p(x_k|z_{1:k},u_{0:k-1})\end{array}$

The conditional probability of the prediction p(x_k|z_1:k−1, u_0:k−1) is therefore normally denoted as

$\begin{array}{l}\hfill be{l}_p(x_k)=p(x_k|z_{1:k-1},u_{0:k-1})\end{array}$

Algorithm 3

Bayesian Filter

The Bayesian filter is estimating the probability distribution of the state estimate. In a moment, when the information about the current action u_k−1 is known, the prediction step can be made, and when the new measurement is available, the correction step can be made. Let us introduce the normalization factor $\eta =\frac{1}{\alpha }=\frac{1}{p(z_k|z_{1:k-1},u_{0:k-1})}$. The Bayesian filter algorithm (Algorithm 3) first calculates the prediction and then the correction that is properly normed.

As it can be observed from Algorithm 3, an integral must be evaluated to determine the probability distribution in the prediction step. Hence the implementation of the Bayesian algorithm is limited to simple continuous cases, where an explicit integral solution can be determined, and discrete cases with countable number of states, where the operation of integration can be substituted for the operation of summation.

Example 6.7

A mobile robot is equipped with a sensor that can detect if the floor is clean or not (Z ∈{clean, dirty}). The mobile system is also equipped with a cleaning system (a set of brushes, a vacuum pump, and a dust bin) that can clean the floor, but the cleaning system is engaged only if the robot believes the floor needs cleaning (U ∈{clean, null}). Again we would like to determine if the floor is clean or not (X ∈{clean, dirty}).

The initial probability (belief) that the floor is clean is

$\begin{array}{l}\hfill bel(X_0=clean)=0.5\end{array}$

The correctness of the sensor measurements is given with a statistical model of the sensor:

$\begin{array}{l}\hfill \begin{array}{cccc}P(Z_k=clean|X_k=clean)& =0.8,& P(Z_k=dirty|X_k=clean)& =0.2\\ P(Z_k=dirty|X_k=dirty)& =0.9,& P(Z_k=clean|X_k=dirty)& =0.1\end{array}\end{array}$

The outcome probabilities if the robot decides to perform floor cleaning are the following:

$\begin{array}{l}\hfill \begin{array}{cc}P(X_k=clean|X_{k-1}=clean,U_{k-1}=clean)& =1\\ P(X_k=dirty|X_{k-1}=clean,U_{k-1}=clean)& =0\\ P(X_k=clean|X_{k-1}=dirty,U_{k-1}=clean)& =0.8\\ P(X_k=dirty|X_{k-1}=dirty,U_{k-1}=clean)& =0.2\end{array}\end{array}$

If the cleaning system is not activated, the following outcome probabilities can be assumed:

$\begin{array}{l}\hfill \begin{array}{cc}P(X_k=clean|X_{k-1}=clean,U_{k-1}=null)& =1\\ P(X_k=dirty|X_{k-1}=clean,U_{k-1}=null)& =0\\ P(X_k=clean|X_{k-1}=dirty,U_{k-1}=null)& =0\\ P(X_k=dirty|X_{k-1}=dirty,U_{k-1}=null)& =1\end{array}\end{array}$

Assume that the mobile system first makes an action and then receives the measurement data. Determine the belief bel_p(x_k) based on the action taken (prediction) and the belief bel(x_k) based on measurement bel(x_k) (correction) for the following sequence of actions and measurements:

$\begin{array}{l}\hfill \begin{array}{ccc}k& U_{k-1}& Z_k\\ 1& null& dirty\\ 2& clean& clean\\ 3& clean& clean\end{array}\end{array}$

Solution

Without loss of generality let us introduce the following notation: X_k ∈{clean, dirty} as $X_k\in \{x_k,{\stackrel{-}{x}}_k\}$, Z_k ∈{clean, dirty} as $Z_k\in \{z_k,{\stackrel{-}{z}}_k\}$, and U_k ∈ (clean, null) as $U_k\in \{u_k,{ū}_k\}$.

Let us use Algorithm 3. In time step k = 1, when the action ${ū}_0=null$ is active we can determine the predicted belief that the floor is clean is the following:

$\begin{array}{l}\hfill \begin{array}{cc}be{l}_p(x_1)& =\sum _{x_0\in X}P(x_1|x_0,{ū}_0)bel(x_0)\\ & =P(x_1|{\stackrel{-}{x}}_0,{ū}_0)bel({\stackrel{-}{x}}_0)+P(x_1|x_0,{ū}_0)bel(x_0)\\ & =0\cdot 0.5+1\cdot 0.5\\ & =0.5\end{array}\end{array}$

and the predicted belief that the floor is dirty:

$\begin{array}{l}\hfill \begin{array}{cc}be{l}_p({\stackrel{-}{x}}_1)& =\sum _{x_0\in X}P({\stackrel{-}{x}}_1|x_0,{ū}_0)bel(x_0)\\ & =P({\stackrel{-}{x}}_1|{\stackrel{-}{x}}_0,{ū}_0)bel({\stackrel{-}{x}}_0)+P({\stackrel{-}{x}}_1|x_0,{ū}_0)bel(x_0)\\ & =1\cdot 0.5+0\cdot 0.5\\ & =0.5\end{array}\end{array}$

Since no action has been taken, the state probabilities are unchanged. Based on the measurement ${\stackrel{-}{z}}_1=dirty$ the belief can be corrected:

$\begin{array}{l}\hfill bel(x_1)=\eta p({\stackrel{-}{z}}_1|x_1)be{l}_p(x_1)=\eta 0.2\cdot 0.5=\eta 0.1\end{array}$

and

$\begin{array}{l}\hfill bel({\stackrel{-}{x}}_1)=\eta p({\stackrel{-}{z}}_1|{\stackrel{-}{x}}_1)be{l}_p({\stackrel{-}{x}}_1)=\eta 0.9\cdot 0.5=\eta 0.45\end{array}$

After the normalization factor η is evaluated:

$\begin{array}{l}\hfill \eta =\frac{1}{0.1+0.45}=1.82\end{array}$

The final values of the belief can be determined:

$\begin{array}{l}\hfill \begin{array}{cc}bel(x_1)=0.182,& bel(\stackrel{-}{x_1})=0.818\end{array}\end{array}$

The procedure can be repeated for time step k = 2, where u₁ = clean and z₂ = clean:

$\begin{array}{l}\hfill \begin{array}{cccc}be{l}_p(x_2)& =0.8364,& be{l}_p({\stackrel{-}{x}}_2)& =0.1636\\ bel(x_2)& =0.9761,& bel({\stackrel{-}{x}}_2)& =0.0239\end{array}\end{array}$

And it can be continued for time step k = 3, where u₂ = clean and z₃ = clean:

$\begin{array}{l}\hfill \begin{array}{cccc}be{l}_p(x_3)& =0.9952,& be{l}_p({\stackrel{-}{x}}_3)& =0.0048\\ bel(x_3)& =0.9994,& bel({\stackrel{-}{x}}_3)& =0.0006\end{array}\end{array}$

The solution of Example 6.7 in Matlab is given in Listing 6.3.

Listing 6.3

Implementation of the solution of Example 6.7

1 %  Notation: X  ===  X (k) , X ’ ===  X (k−1)

2 disp ( ’ Initial belief  of clean and dirtyf l o o r ’)

3 bel_Xc= 0 . 5 ; %  bel(X = clean)

4 bel_X= [ bel_Xc1− bel_Xc ] %  bel(X = clean) , bel(X = dirty)

5

6 disp( ’Conditionalprobabilities  of the dirt sensor measurements ’)

7 P_ZcXc= 0 . 8 ;   %  P (Z = clean|X = clean)

8 P_ZdXc=  1− P_ZcXc ; %  P (Z = dirty|X = clean)

9 P_ZdXd= 0 . 9 ;   %  P (Z = dirty|X = dirty)

10 P_ZcXd=  1− P_ZdXd ; %  P (Z = clean|X = dirty)

11 p_ZX= [ P_ZcXc ,  P_ZcXd; …

12          P_ZdXc ,  P_ZdXd]

13

14 disp( ’Outcomeprobabilities  in the case of cleaning ’)

15 P_XcXcUc= 1;   %  P (X = clean|X ’=clean ,U ’=clean)

16 P_XdXcUc=  1− P_XcXcUc ; %  P (X = dirty|X ’=clean ,U ’=clean)

17 P_XcXdUc= 0 . 8 ;   %  P (X = clean|X ’=dirty ,U ’=clean)

18 P_XdXdUc=  1− P_XcXdUc ; %  P (X = dirty|X ’=dirty ,U ’=clean)

19 p_ZXUc= [P_XcXcUc,  P_XdXcUc; …

20         P_XcXdUc,  P_XdXdUc]

21

22 disp( ’Outcomeprobabilities  in the case of no cleaning actioni s  taken ’)

23 P_XcXcUn= 1;   %  P (X = clean|X ’=clean ,U ’=null)

24 P_XdXcUn=  1− P_XcXcUn ; %  P (X = dirty|X ’=clean ,U ’=null)

25 P_XcXdUn= 0;   %  P (X = clean|X ’=dirty ,U ’=null)

26 P_XdXdUn=  1− P_XcXdUn ; %  P (X = dirty|X ’=dirty ,U ’=null)

27 p_ZXUn= [P_XcXcUn,  P_XdXcUn; …

28         P_XcXdUn,  P_XdXdUn]

29

30 U =  { ’null ’ ,  ’clean ’ ,’clean ’};

31 Z=  { ’dirty ’ ,’clean ’ ,’clean ’};

32 for k=1:length(U)

33     f p r i n t f ( ’Prediction step: U (% d)=% s∖n ’ , k−1, U { k })

34     i f strcmp ( U ( k ) , ’clean ’)

35       belp_X=  bel_X*p_ZXUc

36     else

37       belp_X=  bel_X*p_ZXUn

38     end

39

40     f p r i n t f ( ’Correction step: Z (% d)=% s∖n ’ , k ,  Z{k})

41     if  strcmp(Z(k) ,’clean ’)

42       bel_X=  p_ZX( 1 , : ) .* belp_X ;

43     else

44       bel_X=  p_ZX( 2 , : ) .* belp_X ;

45     end

46     bel_X=  bel_X/sum( bel_X )

47 end

   Initial belief of clean and dirty floor

   bel_X =

      0.5000   0.5000

   Conditional probabilities of the dirt sensor measurements

   p_ZX =

      0.8000   0.1000

      0.2000   0.9000

   Outcome probabilities in the case of cleaning

   p_ZXUc =

      1.0000   0

      0.8000   0.2000

   Outcome probabilities in the case of no cleaning action i s taken

   p_ZXUn =

      1   0

      0   1

   Prediction step : U (0)= null

   belp_X =

      0.5000   0.5000

   Correction step : Z (1)= dirty

   bel_X =

      0.1818   0.8182

   Prediction step : U (1)= clean

   belp_X =

      0.8364   0.1636

   Correction step : Z (2)= clean

   bel_X =

      0.9761   0.0239

   Prediction step : U (2)= clean

   belp_X =

      0.9952   0.0048

   Correction step : Z (3)= clean

   bel_X =

      0.9994   0.0006