卡尔曼滤波 - 状态空间模型

flyfish

状态空间模型是什么和如何得到的状态空间模型
状态方程和观测方程统称为状态空间模型
状态空间模型如下

$x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}$（状态方程)

$z_k=H{x}_k+v_k$观测方程
还可以这样表示只是字母不同
$\mathbf{x}(t+1)=\mathbf{\Phi }\mathbf{x}(t)+\mathbf{\Gamma }w(t)$ （状态方程)
$y(t)=\mathbf{H}\mathbf{x}(t)+v(t)$ （观测方程 )
以物理运动定律的方式来理解状态空间模型
$$\begin{array}{c}s(t+1)=s(t)+\dot{s}(t)T+\frac{T^2}{2}w(t)\\ \dot{s}(t+1)=\dot{s}(t)+Tw(t)\\ y(t)=s(t)+v(t)\end{array}$$
$T$是采样周期
$s(t)$是位置
$\dot{s}(t)$是速度
$w(t)$是加速度
$y(t)$是在位置s(t)的观测信号
$v(t)$是观测噪声

定义系统的状态变量
$$\mathbf{x}(t)=\left[\begin{array}{l}s(t)\\ \dot{s}(t)\end{array}\right]=\left[\begin{array}{l}\text{ 位置 }\\ \text{ 速度 }\end{array}\right]$$
状态方程和观测方程
$$\begin{array}{l}\left[\begin{array}{l}s(t+1)\\ \dot{s}(t+1)\end{array}\right]=\left[\begin{array}{ll}1& T\\ 0& 1\end{array}\right]\left[\begin{array}{l}s(t)\\ \dot{s}(t)\end{array}\right]+\left[\begin{array}{c}\frac{T^2}{2}\\ T\end{array}\right]w(t)\\ y(t)=\left[\begin{array}{ll}1& 0\end{array}\right]\left[\begin{array}{l}s(t)\\ \dot{s}(t)\end{array}\right]+v(t)\end{array}$$
得到 状态空间模型
$\mathbf{x}(t+1)=\mathbf{\Phi }\mathbf{x}(t)+\mathbf{\Gamma }w(t)$ （状态方程)
$y(t)=\mathbf{H}\mathbf{x}(t)+v(t)$ （观测方程 $)$
$$\mathbf{\Phi }=\left[\begin{array}{ll}1& T\\ 0& 1\end{array}\right],\mathbf{\Gamma }=\left[\begin{array}{c}0.5T^2\\ T\end{array}\right],\mathbf{H}=[10]$$

从控制理论来理解状态空间模型

$x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}$

$z_k=H{x}_k+v_k$

这两行式子怎么来的呢

系统动态方程是

$$\begin{array}{c}m\ddot{x}+b\dot{x}+kx=F\\ \text{ 设 }{x}_1=x,\\ x_2=\dot{x},\\ u=F\text{ 则 }\\ \begin{array}{r}{\dot{x}}_2=\ddot{x}=\frac{1}{m}u-\frac{b}{m}\dot{x}-\frac{k}{m}x\\ =\frac{1}{m}u-\frac{b}{m}{x}_2-\frac{k}{m}{x}_1\end{array}\end{array}$$

假设

位置测量量是 $z_1=x=x_1,$

速度测量量是 $z_2=\dot{x}=x_2$, 则:
$$\begin{array}{r}\left[\begin{array}{c}{\dot{x}}_1\\ {\dot{x}}_2\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -\frac{k}{m}& -\frac{b}{m}\end{array}\right]\left[\begin{array}{l}{x}_1\\ x_2\end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{m}\end{array}\right]u\\ \left[\begin{array}{l}{z}_1\\ z_2\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{l}{x}_1\\ x_2\end{array}\right]\end{array}$$

状态空间表达式增加随时间维度
$$\begin{array}{c}\dot{\mathbf{x}}(t)=A\mathbf{x}(t)+Bu(t)\\ \mathbf{z}(t)=H\mathbf{x}(t)\end{array}$$
状态空间表达式将连续的时间更改为离散的时间
$$\begin{array}{c}{\mathbf{x}}_k=A{\mathbf{x}}_{k-1}+B{u}_k\\ {\mathbf{z}}_k=H{\mathbf{x}}_k\end{array}$$
状态空间表达式增加噪声维度 （Linear state space system）
$$\begin{array}{c}{\mathbf{x}}_k=A{\mathbf{x}}_{k-1}+B{u}_k+{\mathbf{w}}_{k-1}\\ {\mathbf{z}}_k=H{\mathbf{x}}_k+{\mathbf{v}}_k\end{array}$$

从速度控制来理解状态方程

连续系统
$$\begin{array}{c}\dot{\mathbf{x}}(t)=A\mathbf{x}(t)+Bu(t)\end{array}$$
离散系统

$${\mathbf{x}}_k=A{\mathbf{x}}_{k-1}+B{u}_k$$

$$\left[\begin{array}{c}po{s}_{k+1}\\ ve{l}_{k+1}\end{array}\right]=\left[\begin{array}{ll}{a}_{11}& a_{12}\\ a_{21}& a_{22}\end{array}\right]\left[\begin{array}{c}po{s}_k\\ ve{l}_k\end{array}\right]+\left[\begin{array}{l}0\\ b_2\end{array}\right]u$$

下一时刻的位置

与当前时刻的位置有关 $a_{1}1$

与当前的速度有关 $a_{1}2$

与当前的输入无关 $b_1=0$

下一时刻的速度

与当前时刻的位置有关 $a21$

与当前的速度有关 $a22$

与当前的输入有关 $b2$
$$A=\left[\begin{array}{ll}{a}_{11}& a_{12}\\ a_{21}& a_{22}\end{array}\right]=\left[\begin{array}{cc}1& 1-0.2\\ -0.2& 1\end{array}\right]$$
下一时刻位置是当前位置+0.8*v

下一时刻速度是当前速度-0.2*p

从实际数值例子来理解

$u(t)$输入量

$\dot{x}(t)$是输出量
下面的$\hat{x}$头上本来写的是三个点，但是这个解析不了所以戴了个帽子
$$\hat{x}+5\ddot{x}+3\dot{x}+2x=u$$

设

$$x_1=x,x_2=\dot{x},x_3=\ddot{x}$$
除最高阶数$\hat{x}$之外，其余各个微分项$x_i$都用替代。
$$\{\begin{array}{l}{\dot{x}}_1=\dot{x}=x_2\\ {\dot{x}}_2=\ddot{x}=x_3\\ {\dot{x}}_3=\hat{x}=-5x_3-3x_2-2x_1+u\end{array}$$
$y$表示输出变量,只有一个输出量$\dot{x}(t)$

$y=x_2=\dot{x}$

变成矩阵形式
$$\begin{array}{l}\left[\begin{array}{c}{\dot{x}}_1\\ {\dot{x}}_2\\ {\dot{x}}_3\end{array}\right]=\left[\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\\ -2& -3& -5\end{array}\right]\left[\begin{array}{l}{x}_1\\ x_2\\ x_3\end{array}\right]+\left[\begin{array}{l}0\\ 0\\ 1\end{array}\right]u\\ [y]=\left[\begin{array}{lll}0& 1& 0\end{array}\right]\left[\begin{array}{l}{x}_1\\ x_2\\ x_3\end{array}\right]+[0]u\end{array}$$
即
$$\begin{array}{l}\dot{\vec{x}}=A\vec{x}+B\vec{u}\\ \vec{y}=C\vec{x}+D\vec{u}\end{array}$$