查看“非线性系统最优估计”的源代码

== 离散非线性系统 ==

=== 问题表述 ===

==== 状态方程和观测方程 ====



假设离散非线性系统的状态方程和观测方程为

\[
\begin{align}

\vec{x}_{k+1} & = \vec{f}(\vec{x}_{k},\vec{u}_k,k) + \vec{w}_{k+1} \\

\vec{z}_k & = \vec{h}(\vec{x}_k,k) + \vec{v}_k

\end{align}

\]



其中，$$\vec{x} _ k$$为$$n$$维状态向量,$$\vec{u}_k$$为$$r$$维控制向量，$$z_k$$为$$m$$维观测向量。$$\vec{w}_k,\vec{w}_k,\vec{x}_0$$为独立的随机变量。$$\vec{w}_k$$和$$\vec{v}_k$$
分别为$$n$$,$$m$$维噪声（一般考虑是零均值高斯白噪声），其概率密度分别为$$P_w(\vec{w}_k)$$和$$P_v(\vec{v}_k)$$。初始状态$$\vec{x}_0$$的概率密度函数为$$P_x(\vec{x}_k)$$。

假设在$$k$$时刻已经获得实时信息$$\mathrm{Z}^*(k)=\{\mathrm{Z}_1^k,\mathrm{U}_1^{k-1}\}$$，其中$$\mathrm{Z}_1^k=\{\vec{z}_1,\cdots,\vec{z}_k\}, \mathrm{U}_1^k=\{\vec{u}_1,\cdots,\vec{u}_k\}$$

==== 贝叶斯估计 ====
贝叶斯估计，即在给定$$k$$时刻实时信息$$\mathrm{Z}^*(k)$$的情况下，求出状态$$\vec{x}_k$$的条件概率密度
\[
p(\vec{x}|\mathrm{Z}^*(k))
\]

$$\vec{x}_k$$的条件期望也就是在实时信息下$$\vec{x}_k$$的最小方差估计，即
$$\newcommand{\d}{\mathrm{d}}$$
\[
\hat{\vec{x}}_{k|k} = E[\vec{x}_k|\mathrm{Z}^*(k)] = \int_{-\infty}^\infty \vec{x}_k p(\vec{x}|\mathrm{Z}^*(k)) \d\vec{x}_k
\]
而估计误差$$ \tilde{\vec{x}}_k \equiv \vec{x}_k - \hat{\vec{x}}_{k|k} $$ 的条件协方差矩阵为

\[
\begin{align}
\mathrm{P}_{k|k} = Cov[\tilde{\vec{x}}_k,\tilde{\vec{x}}_k|\mathrm{Z}^*(k)]
 = \int_{-\infty}^\infty (\vec{x}_k - \hat{\vec{x}}_{k|k})(\vec{x}_k - \hat{\vec{x}}_{k|k})^T p(\vec{x}|\mathrm{Z}^*(k)) \d\vec{x}_k
\end{align}
\]

问题归结于，如何递推地求出条件概率密度$$p(\vec{x}|\mathrm{Z}^*(k))$$


=== 求解 ===

首先根据状态方程和测量方程有：

\[
\begin{align}
&p(\vec{x}_{k+1}|\vec{x}_{k},\vec{u}_{k}) = p_w(\vec{x}_{k+1} - \vec{f}(k,\vec{x}_k,\vec{u}_k)) \\
&p(\vec{z}_{k+1}|\vec{x}_{k+1}) = p_v(\vec{z}_{k+1} - \vec{h}(k+1,\vec{x}_{k+1}))
\end{align}
\]
当进行到第$$k$$步，我们有了$$ \mathrm{Z}^{*}(k) $$和$$ \vec{u}_{k} $$后，可以得到$$\vec{x}_{k+1},\vec{z}_{k+1}$$的条件概率（'''预测'''）：
\[
\begin{align}

&p(\vec{x}_{k+1}|\mathrm{Z}^{*}(k),\vec{u}_{k}) = \int_{-\infty}^\infty p_w(\vec{x}_{k+1} - \vec{f}(k,\vec{x}_k,\vec{u}_k)) p(\vec{x}_{k}|\mathrm{Z}^{*}(k)) \d\vec{x}_{k} \\

&p(\vec{z}_{k+1}|\mathrm{Z}^{*}(k),\vec{u}_k) = \int_{-\infty}^\infty p_v(\vec{z}_{k+1}-\vec{h}(k+1,\vec{x}_{k+1}))p(\vec{x}_{k+1}|\mathrm{Z}^{*}(k),\vec{u}_{k}) \d\vec{x}_{k+1}
\end{align}
\]

在进一步得到测量值$$\vec{z}_{k+1}$$后，可以得到$$\vec{x}_{k+1}$$的后验概率：
\[
\begin{align}
\label{eq:BayesFilter}
{
\begin{aligned}
p(\vec{x}_{k+1}|\mathrm{Z}^{*}(k+1)) &= \frac{p(\vec{x}_{k+1},\vec{z}_{k+1}|\mathrm{Z}^{*}(k),\vec{u}_k)}
{p(\vec{z}_{k+1}|\mathrm{Z}^{*}(k),\vec{u}_k)} \\ 
&=\frac{p_v(\vec{z}_{k+1} - \vec{h}(k+1,\vec{x}_{k+1})) p(\vec{x}_{k+1}|\mathrm{Z}^{*}(k),\vec{u}_{k})}
{\int_{-\infty}^\infty p_v(\vec{z}_{k+1}-\vec{h}(k+1,\vec{x}_{k+1}))p(\vec{x}_{k+1}|\mathrm{Z}^{*}(k),\vec{u}_{k}) \d\vec{x}_{k+1} }
\end{aligned}
}
\end{align}
\]

上式被称为'''贝叶斯滤波公式'''，虽然是直观精确的表达式，但其中每一步的积分都是非常困难的，很可能得不到解析表达式，因而没有重要的应用价值。

== 连续非线性系统 ==

假设系统的状态方程和观测方程为：
\[
\begin{align}
& \dot{\vec{x}}(t) = \vec{f}(\vec{x}(t),t)+\mathrm{g}(\vec{x}(t),t) \vec{w}(t) \\
& \vec{z}(t) = \vec{h}(\vec{x}(t),t) + \vec{v}(t)
\end{align}
\]
其中$$\vec{w}(t)$$和$$\vec{v}(t)$$均为零均值高斯白噪声，且与初始状态$$\vec{x}(t_0)$$不相关，即

\[
\begin{align}
& E[\vec{w}(t)] = E[\vec{v}(t)] = 0 \\
& \Cov[\vec{w}(t),\vec{w}(\tau)] = \mathrm{Q}(t)\delta(t-\tau)\\
& \Cov[\vec{v}(t),\vec{v}(\tau)] = \mathrm{R}(t)\delta(t-\tau) \\
& \Cov[\vec{w}(t),\vec{v}(\tau)] = \Cov[\vec{w}(t),\vec{x}(t_0)] = \Cov[\vec{v}(t),\vec{x}(t_0)] = 0
\end{align}
\]

状态方程和观测方程也可以改写微分形式：
\[
\begin{align}
\label{dynamic}
& \d\vec{x}(t) = \vec{f}(\vec{x}(t),t)\d t+\mathrm{g}(\vec{x}(t),t) \d \vec{N}(t) \\
\label{measure}
& \d\vec{y}(t) = \vec{h}(\vec{x}(t),t)\d t + \d\vec{M}(t)
\end{align}
\]

其中$$\vec{N}(t),\vec{M}(t),\vec{y}(t)$$满足：
\[
\begin{align}
& E[\d\vec{M}(t)] = 0, \Var[\d\vec{M}(t)] = \vec{R}(t)\d t \\
& E[\d\vec{N}(t)] = 0, \Var[\d\vec{N}(t)] = \vec{Q}(t)\d t \\
& \d\vec{y}(t) = \vec{z}(t)\d t
\end{align}
\]

其中\ref{dynamic}是有名的随机微分方程，解为Fokker-Planck方程，于是

\[
\begin{align}
p&(\vec{x},t+\d t|\mathrm{Y}^*(t+\d t))- p(\vec{x},t|\mathrm{Y}^*(t)) \\
=& p(\vec{x},t+\d t|\mathrm{Y}^*(t+\d t))-p(\vec{x},t+\d t|\mathrm{Y}^*(t))+ p(\vec{x},t+\d t|\mathrm{Y}^*(t))- p(\vec{x},t|\mathrm{Y}^*(t)) \\
\label{FP_term}
=&{\color{blue}{ - \sum_i \frac{\partial}{\partial x_i} [f_i(\vec{x},t)p(\vec{x},t|\mathrm{Y}^*(t))] \d t
 + \frac{1}{2} \sum_{ij} \frac{\partial^2}{\partial x_i\partial x_j}\{\mathrm{g(\vec{x}(t),t)}\mathrm{Q(t)}\mathrm{g(\vec{x}(t),t)}^T p(\vec{x},t|\mathrm{Y}^*(t)) \} \d t }} \\
\label{Bayes_term}
 & \color{green}{+ (\vec{h}(\vec{x},t) - E(\vec{h}(\vec{x},t | \mathrm{Y}^*(t)) )^T \mathrm{R}^{-1}(\d\vec{y}(t) - E(\vec{h}(\vec{x},t | \mathrm{Y}^*(t))\d t )  p(\vec{x},t|\mathrm{Y}^*(t)) \} \d t }
\end{align}
\]

其中\ref{FP_term} 项就对应Fokker-Planck方程，\ref{Bayes_term}项是新观测的信息$$ \d\vec{y}(t) $$带来的，可通过贝叶斯条件概率公式取$$ \d t\to0 $$得到，这一项的证明见([https://epubs.siam.org/doi/pdf/10.1137/0302009 Kushner H J,1964])

如果能上式求得$$p(\vec{x}(t),t|\mathrm{Y}^*(t))$$，则可以得到状态$$\vec{x}(t)$$的最小方差估计也就是关于$$\mathrm{Y}^*(t)$$的条件均值。

== 连续—离散型系统 ==
状态方程和观测方程