Graphical State Space Model

ShaoLin Lü GraphOptimization Inc.
Beijing, China
[email protected]

Abstract

In this paper, a new framework, named as graphical state space model, is proposed for the real time optimal estimation of one kind of nonlinear state space model. By discretizing this kind of system model as an equation which can not be solved by Extended Kalman filter, factor graph optimization can outperform Extended Kalman filter in some cases. A simple nonlinear example is given to demonstrate the efficiency of this framework.

Index Terms:

Graphical State Space Model, Graph Optimization, Factor Graph, Kalman Filter, Extended Kalman Filter, Dynamic Bayesian Network, Optimal Estimation

I Introduction

Optimal estimation[1]-[4] is a hit subject for engineers, especially in the science community of GPS, inertial navigation or integrated navigation. In many fields, Kalman filter[5] is the chosen one in the eyes of countless engineers. Recently, factor graph[6]-[7] was frequently used to solve many optimal estimation problems in robotics community. It has been widely admitted that on SLAM, SFM and many other subjects better results can be achieved by factor graph optimization than by Kalman filter. However, these good results usually emerged in the cases of post processing. For real time processing, Kalman filter still occupies the hearts of numerous engineers.

In the last forty years, Probabilistic Graphical Model[8] became popular in both academic world and industry world. Pearl[9] introduced the belief propogation to Bayesian network, which can solve the propability inference problem in complex graphical models. Loeiger[7] discovered sum-product algorithm for factor graph, which motivated great improvements in communication community. Dellaert and Kaess[6] developed gorgeous techniques for factor graph, while numerous applications in the robotics community were stimulated by their research.

Generally, there are two kinds of graphcial models[8]: Bayesian network[9] (Directional Graphical Model) and Markov Field (Undirectional Graphical Model). Both of them can be transformed to factor graph. Then, powerful tools, such like junction tree or other appoximate inference algorithms can be used to perform probabilistic inference on factor graph. According to the pioneer work mentioned above, we have enough tools to solve much more complex graphical models than those solved by the pioneers in 1960s. Kalman filter is regarded as a special simple case of dynamic Bayesian network[10] from the viewpoint of graphical models.

In this paper, a new framework, named as Graphical State Space Model(GSSM), is proposed for the real time optimal estimation of one kind of nonlinear state space model. In the framework of Kalman filter, the continous-time state space model is discretized as a time-series model in the chronological order. Here it is demonstrated that some kind of continous-time model can be discretized as a dynamic Bayesian network. Then, factor graph can be used to represent the density functions. This provides the possibility in the increase of the estimation accuracy. Another merit of this dicretization method is that it can reduct the dimension of the equation needed to be solved. A simple example is used to demonstrate the availibility of the new framework. The clou of this paper is how to organize an equation that Kalman filter can not solve while factor graph optimization can solve.

The paper is orginized as follows. Section I is a brief introduction. In section II, Kalman filter is analyzed from the viewpoint of graphical model. In section III, it is demonstrated what factor graph optimization can do for standard discrete state space model. Grapical state space model is proposed in section IV. A simple example is given to demonstrate the availibility of this powerful model in section V. This paper is concluded in section VI.

II Graphical model viewpoint for Kalman Filter

There are many viewpoints for Kalman filter, such as Bayesian filter, information filter or recursive weighted least squared methods. Before we investigate Kalman filter at the perspective of graphical model, let us review this classical algorithm in the traditional manner.

Besides the famous equations of Kalman filter, the prequisite part is continous-time system theory and its discretization. Many problems in the real world can be formulated as a nonlinear system.

	$\displaystyle\dot{x}=f(x)+B(u)+q$		(1)
	$\displaystyle y=C(x)+r$		(2)

The linearization of the above system can be discribed as follows

	$\displaystyle\dot{x}=Ax+Bu+q$		(3)
	$\displaystyle y=Cx+r$		(4)

where $x\in\mathbb{R}^{n}$ is the system state vector, $y\in\mathbb{R}^{m}$ is the measurement vector, $u\in\mathbb{R}^{l}$ is the control vector. $A$ is an $n\times n$ matrix, which is called the system matrix. $B$ is an $n\times l$ matrix, which is called the input matrix. $C$ is an $m\times n$ matrix, which is called the measurement matrix. $q\sim N(0,Q)$ is the process noise and $r\sim N(0,R)$ is the measurement noise. $Q$ and $R$ are the known covariance matrices.

The above system can be discretized as follows

	$\displaystyle x_{k+1}=F_{k}x_{k}+B_{k}u_{k}+q_{k},F_{k}\approx I_{n\times n}+AT$		(5)
	$\displaystyle y_{k+1}=C_{k+1}x_{k+1}+r_{k}$		(6)

where $x_{k}\in\mathbb{R}^{n}$ is the discrete system state vector, $y_{k+1}\in\mathbb{R}^{m}$ is the discrete measurement vector, $u_{k}\in\mathbb{R}^{l}$ is the discrete control vector. $F_{k}$ is an $n\times n$ matrix, which is called the system matrix or the predition matrix. $T$ is the sample time interval. $B_{k}$ is an $n\times l$ matrix, which is called the input matrix. $C_{k+1}$ is an $m\times n$ matrix, which is called the measurement matrix. $q_{k}\sim N(0,Q_{k})$ is the process noise and $r_{k}\sim N(0,R_{k})$ is the measurement noise. $Q_{k}$ and $R_{k}$ are the known covariance matrices.

The classical Kalman filter can be used to estimate the system described by Equations $(5)\sim(6)$ .
Initialize the estimation,

	$\displaystyle\hat{x}_{0}=E(x_{0})=x_{0}^{+}$		(7)
	$\displaystyle P_{0}=E[(x_{0}-\hat{x}_{0})(x_{0}-\hat{x}_{0})^{T}]$		(8)

Update the prior mean and covariance

	$\displaystyle x_{k+1\|k}=F_{k}x_{k}+B_{k}u_{k}$		(9)
	$\displaystyle P_{k+1\|k}=F_{k}P_{k}F_{k}^{T}+Q_{k}$		(10)

Calculate Kalman gain

\displaystyle K_{k+1}=P_{k+1|k}C_{k+1}^{T}(C_{k+1}P_{k+1|k}C_{k+1}^{T}+R_{k})^{-1}

(11)

Update the posteriori mean and covariance

	$\displaystyle x_{k+1}=x_{k+1\|k}+K_{k+1}(y_{k+1}-C_{k+1}x_{k+1\|k})$		(12)
	$\displaystyle P_{k+1}=(I-K_{k+1}C_{k+1})P_{k+1\|k}$		(13)

To summarize, Kalman filter consists of three steps:
1. Utilize the physical laws to model the system correctly, which goes as Equations $(1)\sim(4)$ .
2. Disretize the system, which goes as Equations $(5)\sim(6)$ .
3. Using the famous Equations $(7)\sim(13)$ to update the mean and covariance of $x_{k}$ in the chronological order. From the viewpoint of graphical models, Kalman filter can be ragarded as a specical case of dynamic Bayesian network.

Refer to caption — Figure 1: State space model representation of Kalman filter.

Probabilistic graphicl model theory consists of three parts[8]: representation, inference and learning. In Fig. 2, the optimization problem is represented as a factor graph. Factor graph [6] is a kind of bipartie graph, $F=(\mathbb{U},\mathbb{V},\mathbb{E})$ , which consists of Factor nodes, Variable nodes and Edges. When inference is done via probabilistic graphicl model theory, Hidden Markov Model assumption [10] for density function is accepted as follows

\displaystyle p(x_{0:N},y_{1:N})=p(x_{0})\prod\limits_{k=1}^{N}p(x_{k}|x_{k}-1)p(y_{k}|x_{k})

(14)

In fact, what Kalman filter do in the process of solving Equation (14) is recursively solving a large block tridiagonal equation as follows

	$\displaystyle P_{w}=diag\begin{bmatrix}P_{0}\\ Q_{0}\\ R_{1}\\ Q_{1}\\ R_{2}\\ \vdots\\ Q_{N-1}\\ R_{N}\\ \end{bmatrix},$
	$\displaystyle\begin{bmatrix}I_{n\times n}&0_{n\times n}&0_{n\times n}&\cdots&0_{n\times n}&0_{m\times n}\\ -F_{0}&I_{n\times n}&0_{n\times n}&\cdots&0_{n\times n}&0_{n\times n}\\ 0_{m\times n}&C_{1}&0_{m\times n}&\cdots&0_{m\times n}&0_{m\times n}\\ 0_{n\times n}&-F_{1}&I_{n\times n}&\cdots&0_{n\times n}&0_{n\times n}\\ 0_{m\times n}&0_{m\times n}&C_{2}&\cdots&0_{m\times n}&0_{m\times n}\\ \vdots&\vdots&\vdots&\ddots&\vdots&\vdots\\ 0_{n\times n}&0_{n\times n}&0_{n\times n}&\cdots&-F_{N-1}&I_{n\times n}\\ 0_{m\times n}&0_{m\times n}&0_{m\times n}&\cdots&0_{m\times n}&C_{N}\end{bmatrix}$
	$\displaystyle\begin{bmatrix}x_{0}\\ x_{1}\\ x_{2}\\ \vdots\\ \vdots\\ x_{N-1}\\ x_{N}\end{bmatrix}=\begin{bmatrix}\hat{x}_{0}^{+}\\ B_{0}u_{0}\\ y_{1}\\ B_{1}u_{1}\\ y_{2}\\ \vdots\\ B_{N-1}u_{N-1}\\ y_{N}\\ \end{bmatrix}$		(15)

At time $k+1$ , probabilistic inference is done on a small factor graph represented by Fig. 4.

According to message-passing algorithm theory[10], Kalman filter can only perform forward directional message passing . The reason is that marginalization, which is discribed by Equations $(12)\sim(13)$ , is done every each time. This means that Kalman filter can not pass the later messages backward to the previous states.

The inference problem represented by Fig. 2 can be perfermed by the solution of the equation as follows

\displaystyle P_{w}=diag\begin{bmatrix}P_{k}\\ Q_{k}\\ R_{k+1}\end{bmatrix},\begin{bmatrix}I_{n\times n}&0_{n\times n}\\ -F_{k}&I_{n\times n}\\ 0_{m\times n}&C_{k+1}\end{bmatrix}\begin{bmatrix}x_{k}\\ x_{k+1}\end{bmatrix}=\begin{bmatrix}\hat{x}_{k}\\ B_{k}u_{k}\\ y_{k+1}\end{bmatrix}

(16)

Kalman filter utilizes Equations $(9)\sim(13)$ to solve Equation (16). Different from factor graph optimization, Kalman filter only give the estimation of $x_{k+1}$ , while factor graph optimization will calculate the estimation of $x_{k+1}$ and $x_{k}$ at the same time.

For factor graph optimization, the above equation can be arranged as three factors [6]:
1. Priori factor, which represents the priori $p(x_{k})$ .
2. Between factor, which represents the transition probability $p(x_{k+1}|x_{k})$ .
3. Measurement factor, which represents the conditional probability $p(y_{k+1}|x_{k+1})$ .

\displaystyle p(x_{k},x_{k+1}|y_{k+1})\propto p(x_{k})p(x_{k+1}|x_{k})p(y_{k+1}|x_{k+1})

(17)

The junction tree can be generated for these factors. The clique is simplely comprised of three factors above. Via this tree, probabilistic inference is performed. From the viewpoint of factor graph optimization, the distributions of $x_{k}$ and $x_{k+1}$ are updated while at the perspective of Kalman filter, only the distribution of $x_{k+1}$ is updated, which is called marginalization.

Obviously, Kalman filter is a specical case of factor graph optimization. First, Kalman filter have no iteration steps. Secondly, Kalman filter does not update the distributions of previous states, which is called smoothing at the perspective of traditional optimal estimation.

III Factor Graph Optimization for State Space model

It should be noted that Equation (15) can be solved easily be factor graph optimization. Assuming that

	$\displaystyle p(x_{0})\propto exp\{-\frac{1}{2}\\|x_{0}-x_{0}^{+}\\|_{P_{0}}\}$		(18)
	$\displaystyle p(x_{k}+1\|x_{k})\propto exp\{-\frac{1}{2}\\|x_{k+1}-F_{k}x_{k}-B_{k}u_{k}\\|_{Q_{k}}\}$		(19)
	$\displaystyle p(y_{k+1}\|x_{k+1}\propto exp\{-\frac{1}{2}\\|y_{k+1}-C_{k}x_{k+1}\\|_{R_{k}+1}\}$		(20)

Then, Equation (15) can be formulated as a weighted least squared problem

	$\displaystyle\mathbb{X}^{MAP}={\mathop{argmin}\limits_{\mathbb{X}}{(\\|x_{0}-x_{0}^{+}\\|_{P_{0}}}}+{\mathop{\sum}\limits_{k}(\\|x_{k+1}-F_{k}x_{k}-B_{k}u_{k}\\|_{Q_{k}}}$
	$\displaystyle+\\|y_{k+1}-C_{k}x_{k+1}\\|_{R_{k+1}}))$		(21)

where $\mathbb{X}=\{x_{k}\},k=0,...,N$ .

Open source library, such as Ceres[11], G2O[12], GTSAM[6], miniSAM[13] and minisam[14], can be easily gotten via internet to solve the above problem. In fact, factor graph optimization can solve both single connected graphical model, such as state space model, and multi connected graphical model. It is a powerful bottle-opener for complex graphical models.

There are two kinds of methods for factor graph optimization: global optimization and sliding window optimization. The difference lies in whether do marginalization or not. Since the dimension of the whole graph increases as time goes on, the amounts of caculation will be out of control if the dimension of the graph is not limited. For this reason, when factor graph optimization is used in real-time processing case, sliding window optimization will be adopted, which can be represented as Fig. 5 and Fig. 6.

The block tridiagonal equation goes as follows

	$\displaystyle P_{w}=diag\begin{bmatrix}P_{k}\\ Q_{k}\\ R_{k+1}\\ Q_{k+1}\\ R_{k+2}\\ \vdots\\ Q_{k+w-1}\\ R_{k+w}\\ \end{bmatrix},X_{w}=\begin{bmatrix}x_{k}\\ x_{k+1}\\ x_{k+2}\\ \vdots\\ \vdots\\ x_{k+w-1}\\ x_{k+w}\end{bmatrix},$
	$\displaystyle b_{w}=\begin{bmatrix}\hat{x}_{k}^{+}\\ B_{k}u_{k}\\ y_{k+1}\\ B_{k+1}u_{k+1}\\ y_{k+2}\\ \vdots\\ B_{k+w-1}u_{k+w-1}\\ y_{k+w}\\ \end{bmatrix},$
	$\displaystyle A_{w}={\small\begin{bmatrix}I_{n\times n}&0_{n\times n}&0_{n\times n}&\cdots&0_{n\times n}&0_{n\times n}\\ -F_{k}&I_{n\times n}&0_{n\times n}&\cdots&0_{n\times n}&0_{n\times n}\\ 0_{m\times n}&C_{k+1}&0_{m\times n}&\cdots&0_{m\times n}&0_{m\times n}\\ 0_{n\times n}&-F_{k+1}&I_{n\times n}&\cdots&0_{n\times n}&0_{n\times n}\\ 0_{m\times n}&0_{m\times n}&C_{k+2}&\cdots&0_{m\times n}&0_{m\times n}\\ \vdots&\vdots&\vdots&\ddots&\vdots&\vdots\\ 0_{n\times n}&0_{n\times n}&0_{n\times n}&\cdots&-F_{k+w-1}&I_{n\times n}\\ 0_{m\times n}&0_{m\times n}&0_{m\times n}&\cdots&0_{m\times n}&C_{k+w}\end{bmatrix}}$
	$\displaystyle A_{w}X_{w}=b_{w}$		(22)

\displaystyle\hat{X}_{w}=(A_{w}^{T}P_{w}^{-1}A_{w})^{-1}A^{T}_{w}P_{w}^{-1}b_{w}

(23)

After the above equation is solved, $x_{k+1}$ should be marginalized out for next measurement update.

This sliding window model just has a bigger dimension than the model discribed in Fig. 4. When factor graph optimization is used to solve Equation (22), its solution accuracy is just like the sliding window filter in theory. It does not use another important merit of factor graph, which is that factor graph can model the system into a multi connected graph.

It should not be igored, what factor graph can do while the traditional space model can not do:
1. Model the system as a multi connected graph.
2. Model the system distributedly not unifiedly.
3. Model one part of the system as constants rather than time-series variables.

These merits are not utilized in Equation (22). If a new framework can be designed to adopt them, maybe factor graph optimization can outperform Kalman filter in real-time processing case.

IV Graphical state space model

It is strongly believed by many engineers that Extended Kalman filter is the best solution for sequential data optimal estimation described by Equations $(1)\sim(6)$ . We totally agree with this opinion. Although graphical models solved by factor graph optimization can be much more complex than space model, factor graph optimization can not outperform Kalman filter when they use the same sequential space model. Another fact which can not be ignored is that factor graph can model the system distributedly. When Kalman filter is used to model the system, there is only one universial state which evolves in the chronological order. If you want better results via factor graph optimization, perhaps you should try a different state space model.

For some continous-time system decribed by Equations $(3)\sim(4)$ , there is a different discretization method. For this kind of system, it can be discretized as another kind of graphical model. Consider the system as follows

	$\displaystyle\begin{bmatrix}\dot{x_{c}}\\ \dot{x_{b}}\end{bmatrix}=\begin{bmatrix}A_{c}&A_{b}\\ 0&0\end{bmatrix}\begin{bmatrix}x_{c}\\ x_{b}\end{bmatrix}+\begin{bmatrix}B\\ 0\end{bmatrix}u+\begin{bmatrix}q\\ 0\end{bmatrix}$		(24)
	$\displaystyle y=\begin{bmatrix}C_{c}&C_{b}\end{bmatrix}\begin{bmatrix}x_{c}\\ x_{b}\end{bmatrix}+r$		(25)

where $[x_{c},x_{b}]^{T}$ is the system state vector, $x_{c}\in\mathbb{R}^{n_{c}},x_{b}\in\mathbb{R}^{n_{b}},n_{c}+n_{b}=n$ . $y\in\mathbb{R}^{m}$ is the measurement vector, $u\in\mathbb{R}^{l}$ is the control vector. $A_{c}$ is an $n_{c}\times n_{c}$ matrix and $A_{b}$ is an $n_{c}\times n_{b}$ matrix. $B$ is an $n_{c}\times l$ matrix, which is called the input matrix. $C_{c}$ is an $m\times n_{c}$ matrix and $C_{b}$ is an $m\times n_{b}$ matrix. $q\sim N(0,Q_{c})$ is the process noise and $r\sim N(0,R)$ is the measurement noise. $Q$ and $R$ are the known covariance matrices.

When Kalman filter is used to estimate the state, $[x_{c},x_{b}]^{T}$ , in the above equations, Equations $(5)\sim(6)$ will be used to discretize Equations $(24)\sim(25)$ . The prediction matrix will be

	$\displaystyle F_{k}\approx I_{n\times n}+AT$
	$\displaystyle A=\begin{bmatrix}A_{c}&A_{b}\\ 0&0\end{bmatrix}$		(26)

Next, Kalman filter will recursively solve Equation (15). Obviously, Kalman filter can not make use of the characteristics of this kind of this system.

Graphical state space model is different with the system model used by Kalman filter. No matter in global or sliding window case, it discretizes the system as a graphical model which has a different topological structure.

The system described by Equations $(24)\sim(25)$ can be discretized as follows

	$\displaystyle x_{c}(k+1)=F_{c}(k)x_{c}(k)+F_{b}(k)x_{b}+B_{k}u_{k}+q_{c}(k)$
	$\displaystyle F_{c}(k)\approx I_{n\times n}+A_{c}T,F_{b}=A_{b}T$		(27)
	$\displaystyle y_{k+1}=C_{c}(k+1)x_{c}(k+1)+C_{b}(k+1)x_{b}+r_{k}$		(28)
	$\displaystyle E(x_{b})=x_{b}^{+}$		(29)

where $x_{c}(k)\in\mathbb{R}^{n_{c}},x_{b}\in\mathbb{R}^{n_{b}},n_{c}+n_{b}=n$ . $y_{k+1}\in\mathbb{R}^{m}$ is the measurement vector, $u_{k}\in\mathbb{R}^{l}$ is the control vector. $F_{c}(k)$ is an $n_{c}\times n_{c}$ matrix and $F_{b}(k)$ is an $n_{c}\times n_{b}$ matrix. $T$ is the sample time interval. $B_{k}$ is an $n_{c}\times l$ matrix, which is called the input matrix. $[C_{c}(k+1)\ C_{b}(k+1)]$ is an $m\times n$ matrix, which is called the measurement matrix. $q_{c}(k)\sim N(0,Q_{k})$ is the process noise and $r_{k}\sim N(0,R_{k})$ is the measurement noise. $Q_{k}$ and $R_{k}$ are the known covariance matrices.

Equation (29) is the dicretization of the state $x_{b}$ . This means that constant states can be modeled as a different form rather than a time-series form. This is the distributed discretization not the unified discretization.

Definitely, Kalman filter can not solve Equations $(27)\sim(29)$ if unknown $x_{b}$ is not discretized as a time-series form. This is the limitation of Kalman filter that it must use time-series forms to represent constant variables. Via the kind of discretization described in Equations $(27)\sim(29)$ , the equation solved by factor graph optimization will be different with Equation (22).

Furthermore, since $x_{b}$ is not discretized as a time-series form, the dimension of the equation is smaller than that of Equation (22). The variables are ${x_{b},{x_{c}(i)},i=k,...,k+w}$ , while if the discretization method of Kalman filter is used, the variables of Equation (22) will be ${{x_{b}(i),x_{c}(i)},i=k,...,k+w}$ . Equation (30) can not be solved by Kalman filter, while factor graph optimization can solve it efficiently. Since the structure of $A_{w}$ is not block tridiagonal any more, Kalman filter can not be used recursively to solve the equation. By the way, the norm function are smaller than that discribed by Equation (21). This makes it possible that factor graph optimization can achieve different results with Kalman filter in real-time cases.

\displaystyle A_{w}=\begin{bmatrix}I_{n_{b}\times n_{b}}&0_{n_{b}\times n_{c}}&0_{n_{b}\times n_{c}}&0_{n_{b}\times n_{c}}&\cdots&0_{n_{b}\times n_{c}}&0_{n_{b}\times n_{c}}\\ 0_{n_{c}\times n_{b}}&I_{n_{c}\times n_{c}}&0_{n_{c}\times n_{c}}&0_{n_{c}\times n_{c}}&\cdots&0_{n_{c}\times n_{c}}&0_{n_{c}\times n_{c}}\\ -F_{b}(k)&-F_{c}(k)&I_{n_{c}\times n_{c}}&0_{n_{c}\times n_{c}}&\cdots&0_{n_{c}\times n_{c}}&0_{n_{c}\times n_{c}}\\ C_{b}(k+1)&0_{m\times n_{c}}&C_{c}(k+1)&0_{m\times n_{c}}&\cdots&0_{m\times n_{c}}&0_{m\times n_{c}}\\ -F_{b}(k+1)&0_{n_{c}\times n_{c}}&-F_{c}(k+1)&I_{n_{c}\times n_{c}}&\cdots&0_{n_{c}\times n_{c}}&0_{n_{c}\times n_{c}}\\ C_{b}(k+2)&0_{m\times n_{c}}&0_{m\times n_{c}}&C_{c}(k+2)&\cdots&0_{m\times n_{c}}&0_{m\times n_{c}}\\ \vdots&\vdots&\vdots&\vdots&\ddots&\vdots&\vdots\\ -F_{b}(k+w-1)&0_{n_{c}\times n_{c}}&0_{n_{c}\times n_{c}}&0_{n_{c}\times n_{c}}&\cdots&-F_{c}(k+w-1)&I_{n_{c}\times n_{c}}\\ C_{b}(k+w)&0_{m\times n_{c}}&0_{m\times n_{c}}&0_{m\times n_{c}}&\cdots&0_{m\times n_{c}}&C_{c}(k+w)\end{bmatrix}

\displaystyle P_{w}=diag\begin{bmatrix}P_{b}\\ P_{c}(k)\\ Q_{c}(k)\\ R_{k+1}\\ Q_{c}(k+1)\\ R_{k+2}\\ \vdots\\ Q_{c}(k+w-1)\\ R_{k+w}\\ \end{bmatrix},X_{w}=\begin{bmatrix}x_{b}\\ x_{c}(k)\\ x_{c}(k+1)\\ x_{c}(k+2)\\ \vdots\\ \vdots\\ x_{c}(k+w-1)\\ x_{c}(k+w)\end{bmatrix},

	$\displaystyle b_{w}=\begin{bmatrix}\hat{x}_{b}^{+}\\ \hat{x}_{c}^{+}(k)\\ B_{k}u_{k}\\ y_{k+1}\\ B_{k+1}u_{k+1}\\ y_{k+2}\\ \vdots\\ B_{k+w-1}u_{k+w-1}\\ y_{k+w}\\ \end{bmatrix},$
	$\displaystyle A_{w}X_{w}=b_{w}$		(30)

Via factor graph optimization, the solution of Equation (30) can be gotten

\displaystyle\hat{X}_{w}=(A_{w}^{T}P_{w}^{-1}A_{w})^{-1}A^{T}_{w}P_{w}^{-1}b_{w}

(31)

It is easy to be caculated that the dimension of the equation is smaller than that dimension if sliding window Kalman filter was used.

TABLE I: Dimension Comparision

Viariable	Dim of $A_{w}$	Dim of $b_{w}$	Dim of $X_{w}$
Sliding window	$(n_{b}+nc+m)w\times$	$(n_{b}+$	$(n_{b}+n_{c})$
Kalman filter	$(n_{b}+n_{c})(w+1)$	$n_{c}+m)w$	$\times(w+1)$
Graphicl State	$(nb+(nc+m)w)$	$n_{b}+$	$n_{b}+$
Space model	$\times(n_{b}+n_{c}(w+1))$	$(n_{c}+m)w$	$n_{c}(w+1)$

Graphical state space model is illustrated with state space form in Fig. 7. It can be also represented in factor graph form as it is illustrated in Fig. 8. Obviously, it is a graphical model with cycles. This means that junction tree algorithm must be used to represent the model with a tree without cycles for exact inference. The estimation will get convergent to a fixed point when the probabilistic inference is being perfermed via the junction tree. It will be demonstrated in the next section that graphical state space model can describe the system more accurately than Extended Kalman filter in some cases.

V A simple example

A simple radar tracking model was usually used to demonstrate how to use Kalman filter[15]. Here it is used to demonstrate the efficiency of the new framework. However, it should be noted that this framework can not be used in all kinds of system!!! For this reason, it is strongly recommended that you must perform simulation before real implementation.

A simple two dimensional radar-tracking problem can be described as follows

	$\displaystyle X=\begin{bmatrix}distance\\ velocity\\ altitude\end{bmatrix}=\begin{bmatrix}x\\ \dot{x}\\ h\end{bmatrix}$
	$\displaystyle\dot{x}=0$		(32)

The measured range

\displaystyle\rho=\sqrt{x^{2}+h^{2}}

(33)

The continous-time system of simple radar tracking is described by

	$\displaystyle\begin{bmatrix}\dot{x}\\ \ddot{x}\\ \dot{h}\end{bmatrix}=\begin{bmatrix}0&1&0\\ 0&0&0\\ 0&0&0\end{bmatrix}\begin{bmatrix}x\\ \dot{x}\\ h\end{bmatrix}+q$		(34)
	$\displaystyle\tilde{\rho}=\begin{bmatrix}\frac{x}{\sqrt{x^{2}+h^{2}}}&0&\frac{h}{\sqrt{x^{2}+h^{2}}}\end{bmatrix}\begin{bmatrix}x\\ \dot{x}\\ h\end{bmatrix}+r$		(35)

When the framework of Kalman filter is used, the discrete-time system goes as follows

	$\displaystyle\begin{bmatrix}x_{k+1}\\ \dot{x}_{k+1}\\ h_{k+1}\end{bmatrix}=\begin{bmatrix}1&T&0\\ 0&1&0\\ 0&0&1\end{bmatrix}\begin{bmatrix}x_{k}\\ \dot{x}_{k}\\ h_{k}\end{bmatrix}+q_{k}$		(36)
	$\displaystyle\tilde{\rho}_{k+1}=\begin{bmatrix}\frac{x_{k+1}}{\sqrt{x^{2}_{k+1}+h^{2}_{k+1}}}&0&\frac{h_{k+1}}{\sqrt{x^{2}_{k+1}+h^{2}_{k+1}}}\end{bmatrix}\begin{bmatrix}x_{k+1}\\ \dot{x}_{k+1}\\ h_{k+1}\end{bmatrix}+r_{k+1}$		(37)

where $T$ is the prediction interval.

If graphical state space model is used, the system state can be modeled distributedly.

\displaystyle\alpha_{k+1}=\frac{x_{k+1}}{\sqrt{x^{2}_{k+1}+h^{2}}},\beta_{k+1}=\frac{h}{\sqrt{x^{2}_{k+1}+h^{2}}}

(38)

The discrete-time system is discribed by

\displaystyle A_{w}=\begin{bmatrix}1&0&0&0&0&\cdots&0&0\\ 0&1&0&0&0&\cdots&0&0\\ 0&0&1&0&0&\cdots&0&0\\ 0&-T&-1&1&0&\cdots&0&0\\ \beta_{k+1}&0&0&\alpha_{k+1}&0&\cdots&0&0\\ 0&-T&0&-1&1&\cdots&0&0\\ \beta_{k+2}&0&0&0&\alpha_{k+2}&\cdots&0&0\\ \vdots&\vdots&\vdots&\vdots&\ddots&\vdots&\vdots&\vdots\\ 0&-T&0&0&0&\cdots&-1&1\\ \beta_{k+w}&0&0&0&0&\cdots&0&\alpha_{k+w}\end{bmatrix}

(39)

\displaystyle P_{w}=diag\begin{bmatrix}P_{h}\\ P_{\dot{x}}\\ P(k)\\ Q(k)\\ R_{k+1}\\ Q(k+1)\\ R_{k+2}\\ \vdots\\ Q(k+w-1)\\ R_{k+w}\\ \end{bmatrix},X_{w}=\begin{bmatrix}h\\ \dot{x}\\ x_{k}\\ x_{k+1}\\ x_{k+2}\\ \vdots\\ \vdots\\ x_{k+w-1}\\ x_{k+w}\end{bmatrix},

	$\displaystyle b_{w}=\begin{bmatrix}\hat{h}^{+}\\ \hat{\dot{x}}^{+}\\ \hat{x}^{+}(k)\\ 0\\ y_{k+1}\\ 0\\ y_{k+2}\\ \vdots\\ 0\\ y_{k+w}\\ \end{bmatrix},$
	$\displaystyle A_{w}X_{w}=b_{w}$		(40)

For Kalman filter, simulation settings are set as follows

	$\displaystyle q_{k}\sim N(0,Q_{k}),Q_{k}=\begin{bmatrix}0.005^{2}&0&0\\ 0&0.005^{2}&0\\ 0&0&0\end{bmatrix}$
	$\displaystyle r_{k}\sim N(0,R_{k}),R_{k}=9$
	$\displaystyle T=0.05,P_{0}=\begin{bmatrix}49&0&0\\ 0&49&0\\ 0&0&49\end{bmatrix},x_{0}=\begin{bmatrix}-100\\ 200\\ 2000\end{bmatrix}$		(41)

For graphical state space model, simulation settings are set as follows

	$\displaystyle T=0.05,Q_{k}=0.005^{2},R_{k}=9,$
	$\displaystyle P_{h}=49,P_{dot}=49,P(0)=49,$
	$\displaystyle x^{+}_{0}=-100,\dot{x}^{+}_{0}=200,h^{+}=2000,w=10$		(42)

Obviously, Kalman filter and graphical state space model adopt the same simulation settings. Simulation results¹¹1The open soure codes for this example can be found at github, https://github.com/shaolinbit/GraphicalStateSpaceModel are demonstrated in Fig. 9 and Fig. 10. For this example, graphical state space model can outperform Kalman filter.

VI Conclution and Future works

In this paper, a new framework, named as graphical state space model, was proposed to estimate the state of a class of nonlinear system. A simple example was used to demonstrate the efficiency of this framework. In some cases, graphical state space model can extract more information than the standard Kalman disrete model. Detailed error analysis should be done in the future. What should be explorered next is that which kind of system can be estimated by this framework.

Acknowledgment

Graphical state space model[16] is patent pending. Thanks for Professor Yulong Huang from Harbin University of Engineering for his kindly invitation.

References

[1] D. Simon, “Optimal State Estimation: Kalman, H-infinity, and Nonlinear Approaches,” John Wiley $\&$ Sons, 2006.
[2] P. Zarchan, H. Musoff, “Fundamentals of Kalman Filtering: A Practical Approach, ” AIAA, 2009.
[3] Y. Huang, Y. Zhang, Y. Zhao, J. A. Chambers, “A Novel Robust Gaussian–Student’s t Mixture Distribution Based Kalman Filter, ” IEEE Trans. Signal Processing, vol. 67, Issue. 13, pp. 3606–3620, 2019.
[4] Y. Huang, Y. Zhang, B. Xu, Z. Wu, J. A. Chambers, “A New Adaptive Extended Kalman Filter for Cooperative Localization, ” IEEE Trans. Aerospace and Electronic System, vol. 54, Issue. 1, pp. 353–368, 2018.
[5] R. E. Kalman, “A new approach to Linear Filtering and Predition Problem,” ASME Trans. J. Basic Engineering, vol. 82, pp. 35–45, 1960.
[6] F. Dellaert, M. Kaess, “ Factor Graph for Robotic Perception,” Foundation and Trend in Robotics, Vol. 6 no. 1-2, pp. 1–139, 2017.
[7] F. R. Kschischang, B. J. Frey, Hans-Andrea Loeliger, “Factor Graphs and the Sum-Product Algorithm,” IEEE Trans. Information Theory, Vol. 47, No. 2, pp. 498–519, 2001.
[8] D. Koller, N. Friedman, “Probabilistic Graphical Models: Principles and Techniques,” Mit Press, 2009.
[9] J. Pearl, “Probabilistic Reasoning in Intelligent Systems: Networks of Plausible Inference,” Morgan Kaufmann, 1988.
[10] K. P. Murphy, “Dynamic Bayesian Networks: Representation, Inference and Learning,” UC Berkeley, 2002.
[11] S. Agarwal, K. Mierle and others, “Ceres Solver,” http://ceres-solver.org.
[12] R, Kümmerle, G. Grisetti, H. Strasdat, K. Konolige, W. Burgard, “ g2o: A General Framework for Graph Optimization,” IEEE Conference on Robotics and Automation, pp. 3607–3613, 2011.
[13] J. Dong, Z. Lv, “miniSAM: A Flexible Factor Graph Non-linear Least Squares Optimization Framework,” arxiv.org/abs/1909.00903, 2019.
[14] S. Lü, “http://github.com/shaolinbit/minisam_lib. ”
[15] R. Labbe, “Kalman and Bayesian Filters in Python,” 2015.
[16] S. Lü, “One kind of Optimal estimation Method and System, ” (in Chinese), Patent Number: 202110333355.2, 2021.