=
C
x
~
k
\begin{cases} \tilde{x}_k=A\tilde{x}_{k-1}+Bu_{k-1}+f\left[ y_{k-1}-\tilde{y}_{k-1} \right]\ \tilde{y}_k=C\tilde{x}_k\\end{cases}
{xk=Axk−1+Buk−1+f[yk−1−yk−1]yk=Cx~k
定义误差向量
e
k
=
x
k
−
x
~
k
e_k=x_k-\tilde{x}_k
ek=xk−x~k，则两系统状态方程相减得
系统误差传递方程
e
k
=
(
A
−
f
C
)
e
k
−
1
系统误差传递方程e_k=\left( A-fC \right) e_{k-1}
系统误差传递方程ek=(A−fC)ek−1
其通解为矩阵指数函数，所以当
A
−
f
C
A-fC
A−fC特征值小于0时，误差向量
e
k
e_k
ek各分量均趋于0，即状态观测器能直接估计原系统状态。
本质上，状态观测器是针对状态空间方程描述的确定系统，从错误的系统状态估计值不断收敛到正确的系统状态估计值的数学模型。
2 状态滤波器
实际的系统往往是随机过程，状态描述为：
{
x
k
=
A
x
k
−
1
B
u
k
−
1
w
k
−
1
y
k
=
C
x
k
v
k
\begin{cases} x_k=Ax_{k-1}+Bu_{k-1}+w_{k-1}\ y_k=Cx_k+v_k\\end{cases}
{xk=Axk−1+Buk−1+wk−1yk=Cxk+vk
其中
w
k
w_k
wk称为过程噪声，主要由系统运行过程中的外力导致，如旋翼飞行器运动过程中受到的阵风；
v
k
v_k
vk称为测量噪声，主要由系统测量过程中测量仪器误差、精度不足导致。
本质上，状态滤波器是针对随机状态空间方程描述的随机系统，从不确定观测中提取信息，得到系统状态最优估计的数学模型。当
w
k
w_k
wk与
v
k
v_k
vk为高斯白噪声且相互独立时，状态滤波器为卡尔曼滤波器。
状态滤波器的核心是通过贝叶斯原理不断调整滤波器增益矩阵，以减小随机干扰，使估计的系统状态趋近于真实状态；而状态观测器的核心是通过极点配置确定观测器增益矩阵，使估计的状态跟踪当前系统的状态。
一般地，状态滤波器性能优于状态观测器，但在实际系统中，若随机噪声非高斯噪声，或有若干非线性环节，按常规噪声模型建立的状态滤波器可能发散，此时则更需要状态观测器的稳定性。
3 卡尔曼滤波器
考虑一个随机系统：
{
x
k
=
A
x
k
−
1
B
u
k
−
1
w
k
−
1
z
k
=
C
x
k
v
k
\begin{cases} x_k=Ax_{k-1}+Bu_{k-1}+w_{k-1}\ z_k=Cx_k+v_k\\end{cases}
{xk=Axk−1+Buk−1+wk−1zk=Cxk+vk
其中
w
k
N
(
0
,
Q
)
w_k~N\left( 0, Q \right)
wk N(0,Q)、
v
k
N
(
0
,
R
)
v_k~N\left( 0, R \right)
vk N(0,R)，且
w
k
w_k
wk与
v
k
v_k
vk相互独立。
忽略噪声可建立该系统的先验数学模型(噪声仅满足随机分布，无法建模)：
状态预测方程
{
x
^
k
−
=
A
x
^
k
−
1
B
u
k
−
1
z
^
k
−
=
C
x
^
k
{\text{状态预测方程}\begin{cases} \hat{x}_{k}^{-}=A\hat{x}_{k-1}+Bu_{k-1}\ \hat{z}_{k}^{-}=C\hat{x}_k\\end{cases}}
状态预测方程{xk−=Axk−1+Buk−1zk−=Cxk
基于上述系统，从贝叶斯方法引出卡尔曼滤波器状态更新方程：
状态更新方程
:
x
^
k
=
x
^
k
−
K
k
(
z
k
−
z
^
k
−
)
{\text{状态更新方程}: \hat{x}_k=\hat{x}_{k}^{-}+K_k\left( z_k-\hat{z}_{k}^{-} \right) }
状态更新方程:xk=xk−+Kk(zk−z^k−)
其中
x
^
k
\hat{x}_k
x^k是后验状态估计，即经过一次修正后，当前系统状态的最优估计值；
x
^
k
−
\hat{x}_{k}^{-}
x^k−为系统状态的先验估计；
z
k
z_k
zk为当前状态的测量样本值；
z
^
k
−
\hat{z}_{k}^{-}
z^k−为当前状态的先验测量值；
z
k
−
z
^
k
−
z_k-\hat{z}_{k}^{-}
zk−z^k−代表实际系统与估计系统间随机误差的影响；
K
k
K_k
Kk权衡先验模型与实测数据间对后验分布的关系，称为卡尔曼增益，显然，
z
k
−
z
^
k
−
z_k-\hat{z}_{k}^{-}
zk−z^k−越小表明实际与预测越接近，则后验分布越趋近于先验分布，
z
k
−
z
^
k
−
z_k-\hat{z}_{k}^{-}
zk−z^k−越大表明预测越不可信，需要通过实测来大幅修正先验分布。
下面要确定
K
k
K_k
Kk使后验模型的修正效果最佳，即
x
^
k
\hat{x}_k
x^k越接近真实值
x
k
{x}_k
xk。定义误差向量
{
后验误差向量
e
k
=
x
k
−
x
^
k
先验误差向量
e
k
−
=
x
k
−
x
^
k
−
\begin{cases} \text{后验误差向量}e_k=x_k-\hat{x}_k\ \text{先验误差向量}e_{k}{-}=x_k-\hat{x}_{k}{-}\\end{cases}
{后验误差向量ek=xk−xk先验误差向量ek−=xk−xk−
定义后验误差协方差矩阵为:
P
k
=
E
(
e
k
e
k
T
)
=
[
σ
e
1
2
σ
e
1
σ
e
2
⋯
σ
e
1
σ
e
n
σ
e
2
σ
e
1
σ
e
2
2
⋯
σ
e
2
σ
e
n
⋮
⋮
⋱
⋮
σ
e
n
σ
e
1
σ
e
n
σ
e
2
⋯
σ
e
n
2
]
P_k=E\left( e_ke_{k}^{T} \right) =\left[ \begin{matrix} \sigma _{e1}^{2}& \sigma _{e1}\sigma _{e2}& \cdots& \sigma _{e1}\sigma _{en}\ \sigma _{e2}\sigma _{e1}& \sigma _{e2}^{2}& \cdots& \sigma _{e2}\sigma _{en}\ \vdots& \vdots& \ddots& \vdots\ \sigma _{en}\sigma _{e1}& \sigma _{en}\sigma _{e2}& \cdots& \sigma _{en}^{2}\\end{matrix} \right]
Pk=E(ekekT)=
σe12σe2σe1⋮σenσe1σe1σe2σe22⋮σenσe2⋯⋯⋱⋯σe1σenσe2σen⋮σen2
同理也有先验误差协方差矩阵
P
k
−
P_{k}^{-}
Pk−。根据最小方差估计原理，设损失函数为
t
r
(
P
k
)
\mathrm{tr}\left( P_k \right)
tr(Pk)，即要求
K
k
=
a
r
g
min
[
t
r
(
P
k
)
]
K_k=\mathrm{arg}\min \left[ \mathrm{tr}\left( P_k \right) \right]
Kk=argmin[tr(Pk)]
将
P
k
P_k
Pk展开为：
上面运用了状态更新方程与状态预测方程，考虑到
e
k
−
e_{k}^{-}
ek−与
v
k
v_k
vk相互独立，则进一步：
P
k
=
(
I
−
K
k
C
)
E
(
e
k
−
e
k
−
T
)
(
I
−
K
k
C
)
T
K
k
E
(
v
k
v
k
T
)
K
k
T
=
(
I
−
K
k
C
)
P
k
−
(
I
−
K
k
C
)
T
K
k
R
K
k
T
\begin{aligned}P_k&=\left( I-K_kC \right) E\left( e_{k}{-}{e_{k}{-}}^T \right) \left( I-K_kC \right) ^T+K_kE\left( v_kv_{k}^{T} \right) K_{k}^{T}\&=\left( I-K_kC \right) P_{k}^{-}\left( I-K_kC \right) T+K_kRK_{k}{T}\end{aligned}
Pk=(I−KkC)E(ek−ek−T)(I−KkC)T+KkE(vkvkT)KkT=(I−KkC)Pk−(I−KkC)T+KkRKkT
令
∂
t
r
(
P
k
)
∂
K
k
=
0
\frac{\partial \mathrm{tr}\left( P_k \right)}{\partial K_k}=0
∂Kk∂tr(Pk)=0，即
卡尔曼增益调整方程
K
k
=
P
k
−
C
T
C
P
k
−
C
T
R
{\text{卡尔曼增益调整方程}K_k=\frac{P_{k}{-}CT}{CP_{k}{-}CT+R}}
卡尔曼增益调整方程Kk=CPk−CT+RPk−CT
将
K
k
K_k
Kk代入后验误差协方差矩阵表达式，即得
协方差更新方程
P
k
=
(
I
−
K
k
H
)
P
k
−
{\text{协方差更新方程}P_k=\left( I-K_kH \right) P_{k}^{-}}
协方差更新方程Pk=(I−KkH)Pk−
要更新
K
k
K_k
Kk，则只需要确定先验误差协方差矩阵
P
k
−
P_{k}^{-}
Pk−。同样地，将
P
k
−
P_{k}^{-}
Pk−展开为
P
k
−
=
E
(
e
k
−
e
k
−
T
)
=
E
[
(
x
k
−
x
^
k
−
)
(
x
k
−
x
^
k
−
)
T
]
=
E
[
(
A
e
k
−
1
w
k
−
1
)
(
A
e
k
−
1
w
k
−
1
)
T
]
=
A
E
(
e
k
−
1
e
k
−
1
T
)
A
T
E
(
w
k
−
1
w
k
−
1
T
)
\begin{aligned}P_{k}^{-}&=E\left( e_{k}{-}e_{k}{-T} \right) \&=E\left[ \left( x_k-\hat{x}_{k}^{-} \right) \left( x_k-\hat{x}_{k}^{-} \right) ^T \right] \&=E\left[ \left( Ae_{k-1}+w_{k-1} \right) \left( Ae_{k-1}+w_{k-1} \right) ^T \right] \&=AE\left( e_{k-1}e_{k-1}^{T} \right) A^T+E\left( w_{k-1}w_{k-1}^{T} \right)\end{aligned}
Pk−=E(ek−ek−T)=E[(xk−xk−)(xk−xk−)T]=E[(Aek−1+wk−1)(Aek−1+wk−1)T]=AE(ek−1ek−1T)AT+E(wk−1wk−1T)
考虑到
e
k
e_k
ek与
w
k
w_k
wk相互独立，进一步得到
协方差预测方程
P
k
−
=
A
P
k
−
1
A
T
Q
{\text{协方差预测方程}P_{k}{-}=AP_{k-1}AT+Q}
协方差预测方程Pk−=APk−1AT+Q
至此，得到卡尔曼滤波的五大基本公式，其中核心方程为基于最小方差估计的卡尔曼增益调整方程，具体工作流程如图所示。
4 具体案例：船舶GPS定位
有一船舶出港沿某直线方向航行，辅助北斗卫星进行定位和测速。假设
① 船舶加速度a
(
t
)
a\left( t \right)
a(t)=机动加速度
u
(
t
)
u\left( t \right)
u(t)+随机加速度
w
(
t
)
w\left( t \right)
w(t)，其中
w
(
t
)
w\left( t \right)
w(t)符合高斯分布；
② GPS观测噪声v
(
t
)
v\left( t \right)
v(t)符合高斯分布。
要求用卡尔曼滤波器估计真实运动轨迹。
解决方案
首先建立随机系统的真实模型。以码头出发点为原点，采样周期(雷达扫描周期)为
T
T
T，用
s
(
k
)
s(k)
s(k)表示船舶在采样时刻
k
T
kT
kT处的真实位置，用
z
(
k
)
z\left( k \right)
z(k)表示在时刻
k
T
kT
kT处的GPS定位观测值，则真实系统输出方程为
z
(
k
)
=
s
(
k
)
v
(
k
)
z(k)=s(k)+v(k)
z(k)=s(k)+v(k)。
记在
k
T
kT
kT时刻处船舶速
s
˙
(
k
)
\dot{s}\left( k \right)
s˙(k)，加速度为
a
(
k
)
a\left( k \right)
a(k)，由匀加速公式有：
s
(
k
1
)
=
s
(
k
)
s
˙
(
k
)
T
1
2
a
(
k
)
T
2
⇒
s
˙
(
k
1
)
=
s
˙
(
k
)
T
a
(
k
)
s\left( k+1 \right) =s\left( k \right) +ṡ\left( k \right) T+\frac{1}{2}a\left( k \right) T^2\\Rightarrow \dot{s}\left( k+1 \right) =ṡ\left( k \right) +Ta\left( k \right)
s(k+1)=s(k)+s˙(k)T+21a(k)T2⇒s˙(k+1)=s˙(k)+Ta(k)
其中
a
(
k
)
=
u
(
k
)
w
(
k
)
a(k)=u(k)+w(k)
a(k)=u(k)+w(k)。
定义在采样时刻
k
T
kT
kT系统状态
x
(
k
)
x\left( k \right)
x(k)为船舶的位置和速度，即
x
(
k
)
=
[
s
(
k
)
s
˙
(
k
)
]
T
x(k)=\left[ \begin{matrix} s(k)& \dot{s}(k)\\end{matrix} \right] ^T
x(k)=[s(k)s˙(k)]T可得到船舶运动的状态空间模型
{
[
s
(
k
1
)
s
˙
(
k
1
)
]
=
[
1
T
0
1
]
[
s
(
k
)
s
˙
(
k
)
]
[
0.5
T
T
]
u
(
k
)
[
0.5
T
T
]
w
(
k
)
z
(
k
)
=
[
1
0
最后
现在正是金三银四的春招高潮，前阵子小编一直在搭建自己的网站，并整理了全套的**【一线互联网大厂Java核心面试题库+解析】：包括Java基础、异常、集合、并发编程、JVM、Spring全家桶、MyBatis、Redis、数据库、中间件MQ、Dubbo、Linux、Tomcat、ZooKeeper、Netty等等**
)
v\left( t \right)
v(t)符合高斯分布。
要求用卡尔曼滤波器估计真实运动轨迹。
解决方案
首先建立随机系统的真实模型。以码头出发点为原点，采样周期(雷达扫描周期)为
T
T
T，用
s
(
k
)
s(k)
s(k)表示船舶在采样时刻
k
T
kT
kT处的真实位置，用
z
(
k
)
z\left( k \right)
z(k)表示在时刻
k
T
kT
kT处的GPS定位观测值，则真实系统输出方程为
z
(
k
)
=
s
(
k
)
v
(
k
)
z(k)=s(k)+v(k)
z(k)=s(k)+v(k)。
记在
k
T
kT
kT时刻处船舶速
s
˙
(
k
)
\dot{s}\left( k \right)
s˙(k)，加速度为
a
(
k
)
a\left( k \right)
a(k)，由匀加速公式有：
s
(
k
1
)
=
s
(
k
)
s
˙
(
k
)
T
1
2
a
(
k
)
T
2
⇒
s
˙
(
k
1
)
=
s
˙
(
k
)
T
a
(
k
)
s\left( k+1 \right) =s\left( k \right) +ṡ\left( k \right) T+\frac{1}{2}a\left( k \right) T^2\\Rightarrow \dot{s}\left( k+1 \right) =ṡ\left( k \right) +Ta\left( k \right)
s(k+1)=s(k)+s˙(k)T+21a(k)T2⇒s˙(k+1)=s˙(k)+Ta(k)
其中
a
(
k
)
=
u
(
k
)
w
(
k
)
a(k)=u(k)+w(k)
a(k)=u(k)+w(k)。
定义在采样时刻
k
T
kT
kT系统状态
x
(
k
)
x\left( k \right)
x(k)为船舶的位置和速度，即
x
(
k
)
=
[
s
(
k
)
s
˙
(
k
)
]
T
x(k)=\left[ \begin{matrix} s(k)& \dot{s}(k)\\end{matrix} \right] ^T
x(k)=[s(k)s˙(k)]T可得到船舶运动的状态空间模型
{
[
s
(
k
1
)
s
˙
(
k
1
)
]
=
[
1
T
0
1
]
[
s
(
k
)
s
˙
(
k
)
]
[
0.5
T
T
]
u
(
k
)
[
0.5
T
T
]
w
(
k
)
z
(
k
)
=
[
1
0
最后
现在正是金三银四的春招高潮，前阵子小编一直在搭建自己的网站，并整理了全套的**【一线互联网大厂Java核心面试题库+解析】：包括Java基础、异常、集合、并发编程、JVM、Spring全家桶、MyBatis、Redis、数据库、中间件MQ、Dubbo、Linux、Tomcat、ZooKeeper、Netty等等**
[外链图片转存中…(img-SkyVPqpl-1714715930394)]
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
