卡尔曼滤波器
首先对包含系统白噪声的动态方程进行一般化描述。在离散时域内，线性系统的状态方程如下所示：
{
动态方程：
X
(
k
+
1
)
=
A
X
(
k
)
+
B
U
(
k
)
+
w
观测方程：
Y
(
k
+
1
)
=
C
X
(
k
+
1
)
+
v
\left\{ \begin{array}{l} 动态方程：X\left( {k + 1} \right) = AX\left( k \right) + BU\left( k \right)+ \bf{w}\\ 观测方程：Y\left( k + 1 \right) = CX\left( k + 1 \right) + \bf{v} \end{array} \right.
{动态方程：X(k+1)=AX(k)+BU(k)+w观测方程：Y(k+1)=CX(k+1)+v
其中， U ( k ) U\left( k \right) U(k)为系统输入向量， Y ( k ) Y\left( k \right) Y(k)为系统观测输出向量， X ( k ) X\left( k \right) X(k)为系统状态向量， A A A被定义为状态转移矩阵， B B B被定义为输入矩阵， C C C被定义为观测矩阵， w \bf{w} w为协方差为 Q Q Q的过程噪声向量， v \bf{v} v为协方差为 R R R的观测噪声向量。
已知
k
+
1
k+1
k+1时刻的观测信息以及测量信息，对
k
+
1
k+1
k+1时刻状态进行最优估计
X
^
(
k
+
1
∣
k
+
1
)
\hat X\left( {k+1|k+1} \right)
X^(k+1∣k+1)，以尽可能贴近
k
+
1
k+1
k+1时刻的真实状态
X
(
k
+
1
)
X\left( k+1 \right)
X(k+1)，该优化问题可以数学表示为：
min
J
=
E
{
[
X
(
k
+
1
)
−
X
^
(
k
+
1
∣
k
+
1
)
]
T
[
X
(
k
+
1
)
−
X
^
(
k
+
1
∣
k
+
1
)
]
}
\min\quad J = E\left\{ {{{\left[ {X\left( k+1 \right) - \hat X\left( {k+1|k+1} \right)} \right]}^T}\left[ {X\left( k+1 \right) - \hat X\left( {k+1|k+1} \right)} \right]} \right\}
minJ=E{[X(k+1)−X^(k+1∣k+1)]T[X(k+1)−X^(k+1∣k+1)]}
根据卡尔曼滤波理论，上述优化问题的解可以被表示为：
{
1
先验估计：
X
^
(
k
+
1
∣
k
)
=
A
X
^
(
k
∣
k
)
+
B
U
(
k
)
2
先验方差：
P
(
k
+
1
∣
k
)
=
A
P
(
k
∣
k
)
A
T
+
Q
3
融合增益：
K
(
k
+
1
)
=
P
(
k
+
1
∣
k
)
C
T
[
C
P
(
k
+
1
∣
k
)
C
T
+
R
]
−
1
4
后验估计：
X
^
(
k
+
1
∣
k
+
1
)
=
X
^
(
k
+
1
∣
k
)
+
K
(
k
+
1
)
[
Y
(
k
+
1
)
−
C
X
^
(
k
+
1
∣
k
)
]
5
后验方差：
P
(
k
+
1
∣
k
+
1
)
=
[
I
−
K
(
k
+
1
)
C
]
P
(
k
+
1
∣
k
)
\left\{ \begin{array}{l} 1先验估计：\hat X\left( {k + 1|k} \right) = A\hat X\left( {k|k} \right)+ BU\left( k \right)\\ 2先验方差：P\left( {k + 1|k} \right) = AP\left( {k|k} \right){A^T} + Q\\ 3融合增益：K\left( {k + 1} \right) = P\left( {k + 1|k} \right){C^T}{\left[ {CP\left( {k + 1|k} \right){C^T} + R} \right]^{ - 1}}\\ 4后验估计：\hat X\left( {k + 1|k + 1} \right) = \hat X\left( {k + 1|k} \right) + K\left( {k + 1} \right)\left[ {Y\left( {k + 1} \right) - C\hat X\left( {k + 1|k} \right)} \right]\\ 5后验方差：P\left( {k + 1|k + 1} \right) = \left[ {I - K\left( {k + 1} \right)C} \right]P\left( {k + 1|k} \right) \end{array} \right.
⎩
⎨
⎧1先验估计：X^(k+1∣k)=AX^(k∣k)+BU(k)2先验方差：P(k+1∣k)=AP(k∣k)AT+Q3融合增益：K(k+1)=P(k+1∣k)CT[CP(k+1∣k)CT+R]−14后验估计：X^(k+1∣k+1)=X^(k+1∣k)+K(k+1)[Y(k+1)−CX^(k+1∣k)]5后验方差：P(k+1∣k+1)=[I−K(k+1)C]P(k+1∣k)
其中，
K
(
k
+
1
)
K(k+1)
K(k+1)为
k
+
1
k+1
k+1时刻的融合增益矩阵，
X
^
(
k
+
1
∣
k
)
\hat X\left( {k + 1|k} \right)
X^(k+1∣k)为基于
k
k
k时刻信息与历史信息对
k
+
1
k+1
k+1时刻状态的估计，也被称做
k
+
1
k+1
k+1时刻的先验估计，
X
^
(
k
+
1
∣
k
+
1
)
\hat X\left( {k + 1|k+1} \right)
X^(k+1∣k+1)为基于
k
+
1
k+1
k+1时刻信息与历史信息对
k
+
1
k+1
k+1时刻状态的估计，也被称作
k
+
1
k+1
k+1时刻的后验估计，
P
(
k
+
1
∣
k
)
P\left( {k + 1|k} \right)
P(k+1∣k)和
P
(
k
+
1
∣
k
+
1
)
P\left( {k + 1|k+1} \right)
P(k+1∣k+1)分别表示先验估计误差协方差矩阵与后验估计误差协方差矩阵，其定义如下：
{
P
(
k
+
1
∣
k
)
=
E
{
[
X
(
k
+
1
)
−
X
^
(
k
+
1
∣
k
)
]
[
X
(
k
+
1
)
−
X
^
(
k
+
1
∣
k
)
]
T
}
P
(
k
+
1
∣
k
+
1
)
=
E
{
[
X
(
k
+
1
)
−
X
^
(
k
+
1
∣
k
+
1
)
]
[
X
(
k
+
1
)
−
X
^
(
k
+
1
∣
k
+
1
)
]
T
}
\left\{ \begin{array}{l} P\left( {k+1|k } \right) = E\left\{ {\left[ {X\left( k + 1\right) - \hat X\left( {k + 1|k} \right)} \right]{{\left[ {X\left( k+ 1 \right) - \hat X\left( {k + 1|k} \right)} \right]}^T}} \right\}\\ P\left( {k+1|k+1} \right) = E\left\{ {\left[ {X\left( k+1 \right) - \hat X\left( {k+1|k+1} \right)} \right]{{\left[ {X\left( k+1 \right) - \hat X\left( {k+1|k+1} \right)} \right]}^T}} \right\} \end{array} \right.
⎩
⎨
⎧P(k+1∣k)=E{[X(k+1)−X^(k+1∣k)][X(k+1)−X^(k+1∣k)]T}P(k+1∣k+1)=E{[X(k+1)−X^(k+1∣k+1)][X(k+1)−X^(k+1∣k+1)]T}
值得注意的是，在 k = 0 k=0 k=0时，需要对 X ^ ( 0 ∣ 0 ) \hat X\left( {0|0} \right) X^(0∣0)和 P ( 0 ∣ 0 ) P\left( {0|0} \right) P(0∣0)的初始值进行设置，可取 X ^ ( 1 ∣ 0 ) = C − 1 Y ( 1 ) \hat X\left( {1|0} \right) = {C^{ - 1}}Y\left( 1 \right) X^(1∣0)=C−1Y(1)， P ( 0 ∣ 0 ) = 0 P\left( {0|0} \right) = {\bf{0}} P(0∣0)=0。
扩展卡尔曼滤波器
上述线性卡尔曼滤波器可以实现对线性方程组的最优估计，但对于非线性方程是行不通的。这是因为对先验方差的计算依赖于状态转移矩阵，同样地，对后验方差的计算又依赖于观测矩阵。当然，对融合增益的计算也离不开增益矩阵。
{
动态方程：
X
(
k
+
1
)
=
f
[
X
(
k
)
]
+
w
观测方程：
Y
(
k
+
1
)
=
h
[
X
(
k
+
1
)
]
+
v
\left\{ {\begin{array}{l} 动态方程：X\left( {k + 1} \right) = f\left[ {X\left( k \right)} \right] + {\bf{w}}\\ 观测方程：Y\left( {k + 1} \right) = h\left[ {X\left( {k + 1} \right)} \right] + {\bf{v}} \end{array}} \right.
{动态方程：X(k+1)=f[X(k)]+w观测方程：Y(k+1)=h[X(k+1)]+v
一种比较直观的做法就是，在所关注的点上进行泰勒展开，对非线性方程进行局部线性化，从而得到局部线性化的状态转移矩阵和观测矩阵，完成先验方差、后验方差、融合增益的计算。
{
动态方程
{
X
(
k
+
1
)
=
f
[
X
(
k
)
]
+
w
,
E
x
t
e
n
d
a
t
X
0
=
f
[
X
0
]
+
∂
f
∂
X
∣
X
=
X
0
[
X
(
k
)
−
X
0
]
+
w
,
S
e
l
e
c
t
X
0
=
X
^
(
k
∣
k
)
=
f
[
X
^
(
k
∣
k
)
]
+
∂
f
∂
X
∣
X
=
X
^
(
k
∣
k
)
[
X
(
k
)
−
X
^
(
k
∣
k
)
]
+
w
=
A
X
(
k
)
+
f
[
X
^
(
k
∣
k
)
]
−
A
X
^
(
k
∣
k
)
+
w
A
=
∂
f
∂
X
∣
X
=
X
^
(
k
∣
k
)
=
[
∂
f
1
∂
x
1
∂
f
1
∂
x
2
⋯
∂
f
1
∂
x
n
∂
f
2
∂
x
1
∂
f
2
∂
x
2
⋯
∂
f
2
∂
x
n
⋮
⋮
⋱
⋮
∂
f
n
∂
x
1
∂
f
n
∂
x
2
⋯
∂
f
n
∂
x
n
]
∣
X
=
X
^
(
k
∣
k
)
观测方程
{
Y
(
k
+
1
)
=
h
[
X
(
k
+
1
)
]
+
v
,
E
x
t
e
n
d
a
t
X
0
=
h
[
X
0
]
+
∂
h
∂
X
∣
X
=
X
0
[
X
(
k
+
1
)
−
X
0
]
+
v
,
S
e
l
e
c
t
X
0
=
X
^
(
k
+
1
∣
k
)
=
h
[
X
^
(
k
+
1
∣
k
)
]
+
∂
h
∂
X
∣
X
=
X
^
(
k
+
1
∣
k
)
[
X
(
k
+
1
)
−
X
^
(
k
+
1
∣
k
)
]
+
v
=
C
X
(
k
+
1
)
+
h
[
X
^
(
k
+
1
∣
k
)
]
−
C
X
^
(
k
+
1
∣
k
)
+
v
C
=
∂
h
∂
X
∣
X
=
X
^
(
k
+
1
∣
k
)
=
[
∂
h
1
∂
x
1
∂
h
1
∂
x
2
⋯
∂
h
1
∂
x
n
∂
h
2
∂
x
1
∂
h
2
∂
x
2
⋯
∂
h
2
∂
x
n
⋮
⋮
⋱
⋮
∂
h
n
∂
x
1
∂
h
n
∂
x
2
⋯
∂
h
n
∂
x
n
]
∣
X
=
X
^
(
k
∣
k
)
\left\{ {\begin{array}{l} {动态方程\left\{ \begin{aligned} X\left( {k + 1} \right) &= f\left[ {X\left( k \right)} \right] + {\bf{w}},\ Extend \ at\ X_0\\ &= f\left[ {{X_0}} \right] + {\left. {\frac{{\partial f}}{{\partial X}}} \right|_{X = {X_0}}}\left[ {X\left( k \right) - {X_0}} \right] + {\bf{w}},\ Select\ {X_0} = \hat X\left( {k|k} \right)\\ &= f\left[ {\hat X\left( {k|k} \right)} \right] + {\left. {\frac{{\partial f}}{{\partial X}}} \right|_{X = \hat X\left( {k|k} \right)}}\left[ {X\left( k \right) - \hat X\left( {k|k} \right)} \right] + {\bf{w}}\\ &= AX\left( k \right) + f\left[ {\hat X\left( {k|k} \right)} \right] - A\hat X\left( {k|k} \right) + {\bf{w}}\\ A &= {\left. {\frac{{\partial f}}{{\partial X}}} \right|_{X = \hat X\left( {k|k} \right)}} = {\left. {\left[ {\begin{array}{c} {{\textstyle{{\partial {f_1}} \over {\partial {x_1}}}}}&{{\textstyle{{\partial {f_1}} \over {\partial {x_2}}}}}& \cdots &{{\textstyle{{\partial {f_1}} \over {\partial {x_n}}}}}\\ {{\textstyle{{\partial {f_2}} \over {\partial {x_1}}}}}&{{\textstyle{{\partial {f_2}} \over {\partial {x_2}}}}}& \cdots &{{\textstyle{{\partial {f_2}} \over {\partial {x_n}}}}}\\ \vdots & \vdots & \ddots & \vdots \\ {{\textstyle{{\partial {f_n}} \over {\partial {x_1}}}}}&{{\textstyle{{\partial {f_n}} \over {\partial {x_2}}}}}& \cdots &{{\textstyle{{\partial {f_n}} \over {\partial {x_n}}}}} \end{array}} \right]} \right|_{X = \hat X\left( {k|k} \right)}} \end{aligned} \right.}\\ {观测方程\left\{ \begin{aligned} Y\left( {k + 1} \right) &=h\left[ {X\left( {k + 1} \right)} \right] + {\bf{v}},\ Extend \ at\ X_0\\ &= h\left[ {{X_0}} \right] + {\left. {\frac{{\partial h}}{{\partial X}}} \right|_{X = {X_0}}}\left[ {X\left( {k + 1} \right) - {X_0}} \right] + {\bf{v}},\ Select\ {X_0} = \hat X\left( {k + 1|k} \right)\\ &= h\left[ {\hat X\left( {k + 1|k} \right)} \right] + {\left. {\frac{{\partial h}}{{\partial X}}} \right|_{X = \hat X\left( {k + 1|k} \right)}}\left[ {X\left( {k + 1} \right) - \hat X\left( {k + 1|k} \right)} \right] + {\bf{v}}\\ &= CX\left( {k + 1} \right) + h\left[ {\hat X\left( {k + 1|k} \right)} \right] - C\hat X\left( {k + 1|k} \right) + {\bf{v}}\\ C &= {\left. {\frac{{\partial h}}{{\partial X}}} \right|_{X = \hat X\left( {k + 1|k} \right)}} = {\left. {\left[ {\begin{array}{c} {{\textstyle{{\partial {h_1}} \over {\partial {x_1}}}}}&{{\textstyle{{\partial {h_1}} \over {\partial {x_2}}}}}& \cdots &{{\textstyle{{\partial {h_1}} \over {\partial {x_n}}}}}\\ {{\textstyle{{\partial {h_2}} \over {\partial {x_1}}}}}&{{\textstyle{{\partial {h_2}} \over {\partial {x_2}}}}}& \cdots &{{\textstyle{{\partial {h_2}} \over {\partial {x_n}}}}}\\ \vdots & \vdots & \ddots & \vdots \\ {{\textstyle{{\partial {h_n}} \over {\partial {x_1}}}}}&{{\textstyle{{\partial {h_n}} \over {\partial {x_2}}}}}& \cdots &{{\textstyle{{\partial {h_n}} \over {\partial {x_n}}}}} \end{array}} \right]} \right|_{X = \hat X\left( {k|k} \right)}} \end{aligned} \right.} \end{array}} \right.
⎩
⎨
⎧动态方程⎩
⎨
⎧X(k+1)A=f[X(k)]+w, Extend at X0=f[X0]+∂X∂f
X=X0[X(k)−X0]+w, Select X0=X^(k∣k)=f[X^(k∣k)]+∂X∂f
X=X^(k∣k)[X(k)−X^(k∣k)]+w=AX(k)+f[X^(k∣k)]−AX^(k∣k)+w=∂X∂f
X=X^(k∣k)=
∂x1∂f1∂x1∂f2⋮∂x1∂fn∂x2∂f1∂x2∂f2⋮∂x2∂fn⋯⋯⋱⋯∂xn∂f1∂xn∂f2⋮∂xn∂fn
X=X^(k∣k)观测方程⎩
⎨
⎧Y(k+1)C=h[X(k+1)]+v, Extend at X0=h[X0]+∂X∂h
X=X0[X(k+1)−X0]+v, Select X0=X^(k+1∣k)=h[X^(k+1∣k)]+∂X∂h
X=X^(k+1∣k)[X(k+1)−X^(k+1∣k)]+v=CX(k+1)+h[X^(k+1∣k)]−CX^(k+1∣k)+v=∂X∂h
X=X^(k+1∣k)=
∂x1∂h1∂x1∂h2⋮∂x1∂hn∂x2∂h1∂x2∂h2⋮∂x2∂hn⋯⋯⋱⋯∂xn∂h1∂xn∂h2⋮∂xn∂hn
X=X^(k∣k)
上述过程通过一阶泰勒展开实现了局部线性化，从而得到了状态转移矩阵和观测矩阵，完整的EKF过程如下所示。值得注意的是，一个辅助步骤对应解决一个非线性，如果动态方程或观测方程中某一个本身就是线性的，则可以省去该方程对应的辅助步骤。
{
1
先验估计
:
X
^
(
k
+
1
∣
k
)
=
f
[
X
^
(
k
∣
k
)
]
辅助步骤
:
A
=
∂
f
∂
X
∣
X
=
X
^
(
k
∣
k
)
2
先验方差
:
P
(
k
+
1
∣
k
)
=
A
P
(
k
∣
k
)
A
T
+
Q
辅助步骤
:
C
=
∂
h
∂
X
∣
X
=
X
^
(
k
+
1
∣
k
)
3
融合增益
:
K
(
k
+
1
)
=
P
(
k
+
1
∣
k
)
C
T
[
C
P
(
k
+
1
∣
k
)
C
T
+
R
]
−
1
4
后验估计
:
X
^
(
k
+
1
∣
k
+
1
)
=
X
^
(
k
+
1
∣
k
)
+
K
(
k
+
1
)
[
Y
(
k
+
1
)
−
h
[
X
^
(
k
+
1
∣
k
)
]
]
5
后验方差
:
P
(
k
+
1
∣
k
+
1
)
=
[
I
−
K
(
k
+
1
)
C
]
P
(
k
+
1
∣
k
)
\left\{ {\begin{array}{l} 1先验估计:\hat X\left( {k + 1|k} \right) = f\left[ {\hat X\left( {k|k} \right)} \right]\\ 辅助步骤:A = {\left. {{\textstyle{{\partial f} \over {\partial X}}}} \right|_{X = \hat X\left( {k|k} \right)}}\\ 2先验方差:P\left( {k + 1|k} \right) = AP\left( {k|k} \right){A^T} + Q\\ 辅助步骤:C = {\left. {{\textstyle{{\partial h} \over {\partial X}}}} \right|_{X = \hat X\left( {k + 1|k} \right)}}\\ 3融合增益:K\left( {k + 1} \right) = P\left( {k + 1|k} \right){C^T}{{\left[ {CP\left( {k + 1|k} \right){C^T} + R} \right]}^{ - 1}}\\ 4后验估计:\hat X\left( {k + 1|k + 1} \right) = \hat X\left( {k + 1|k} \right) + K\left( {k + 1} \right)\left[ {Y\left( {k + 1} \right) - h\left[ {\hat X\left( {k + 1|k} \right)} \right]} \right]\\ 5后验方差:P\left( {k + 1|k + 1} \right) = \left[ {I - K\left( {k + 1} \right)C} \right]P\left( {k + 1|k} \right) \end{array}} \right.
⎩
⎨
⎧1先验估计:X^(k+1∣k)=f[X^(k∣k)]辅助步骤:A=∂X∂f
X=X^(k∣k)2先验方差:P(k+1∣k)=AP(k∣k)AT+Q辅助步骤:C=∂X∂h
X=X^(k+1∣k)3融合增益:K(k+1)=P(k+1∣k)CT[CP(k+1∣k)CT+R]−14后验估计:X^(k+1∣k+1)=X^(k+1∣k)+K(k+1)[Y(k+1)−h[X^(k+1∣k)]]5后验方差:P(k+1∣k+1)=[I−K(k+1)C]P(k+1∣k)
无迹卡尔曼滤波器
另一种跨越非线性的方法是暴力粒子群，即用一堆点去近似概率分布
(
X
∼
P
)
\left( {X \sim P} \right)
(X∼P)，然后逐个计算所有点经过非线性函数后的值，并重新进行统计均值和方差。这种方法回归了统计学的本质，但是用一堆点计算是不现实的，因此考虑用少数点，并配合权重的方式达到类似效果，如下所示：
{
辅助步骤
:
X
^
(
k
∣
k
)
,
P
(
k
∣
k
)
→
X
^
(
i
)
(
k
∣
k
)
,
w
m
(
i
)
→
X
^
(
i
)
(
k
+
1
∣
k
)
,
w
m
(
i
)
1
先验估计
:
X
^
(
k
+
1
∣
k
)
=
∑
w
m
(
i
)
X
^
(
i
)
(
k
+
1
∣
k
)
2
先验方差
:
P
(
k
+
1
∣
k
)
=
∑
w
c
(
i
)
[
X
^
(
i
)
(
k
+
1
∣
k
)
−
X
^
(
k
+
1
∣
k
)
]
[
X
^
(
i
)
(
k
+
1
∣
k
)
−
X
^
(
k
+
1
∣
k
)
]
T
+
Q
辅助步骤
:
{
X
^
(
k
+
1
∣
k
)
,
P
(
k
+
1
∣
k
)
→
X
^
(
i
)
(
k
+
1
∣
k
)
,
w
m
(
i
)
→
Y
^
(
i
)
(
k
+
1
∣
k
)
,
w
m
(
i
)
Y
^
(
k
+
1
∣
k
)
=
∑
w
m
(
i
)
Y
^
(
i
)
(
k
+
1
∣
k
)
P
Y
Y
=
∑
w
c
(
i
)
[
Y
^
(
i
)
(
k
+
1
∣
k
)
−
Y
^
(
k
+
1
∣
k
)
]
[
Y
^
(
i
)
(
k
+
1
∣
k
)
−
Y
^
(
k
+
1
∣
k
)
]
T
+
R
P
X
Y
=
∑
w
c
(
i
)
[
X
^
(
i
)
(
k
+
1
∣
k
)
−
X
^
(
k
+
1
∣
k
)
]
[
Y
^
(
i
)
(
k
+
1
∣
k
)
−
Y
^
(
k
+
1
∣
k
)
]
T
3
融合增益
:
K
(
k
+
1
)
=
P
X
Y
P
Y
Y
−
1
4
后验估计
:
X
^
(
k
+
1
∣
k
+
1
)
=
X
^
(
k
+
1
∣
k
)
+
K
(
k
+
1
)
[
Y
(
k
+
1
)
−
Y
^
(
k
+
1
∣
k
)
]
5
后验方差
:
P
(
k
+
1
∣
k
+
1
)
=
P
(
k
+
1
∣
k
)
−
K
(
k
+
1
)
P
Y
Y
K
T
(
k
+
1
)
\left\{ {\begin{array}{l} {辅助步骤:\hat X\left( {k|k} \right),P\left( {k|k} \right) \to {{\hat X}^{\left( i \right)}}\left( {k|k} \right),{w_m}^{\left( i \right)} \to {{\hat X}^{\left( i \right)}}\left( {k + 1|k} \right),{w_m}^{\left( i \right)}}\\ {1先验估计:\hat X\left( {k + 1|k} \right) = \sum {{w_m}^{\left( i \right)}{{\hat X}^{\left( i \right)}}\left( {k + 1|k} \right)} }\\ {2先验方差:P\left( {k + 1|k} \right) = \sum {{w_c}^{\left( i \right)}\left[ {{{\hat X}^{\left( i \right)}}\left( {k + 1|k} \right) - \hat X\left( {k + 1|k} \right)} \right]} {{\left[ {{{\hat X}^{\left( i \right)}}\left( {k + 1|k} \right) - \hat X\left( {k + 1|k} \right)} \right]}^T} + Q}\\ {辅助步骤:\left\{ \begin{array}{l} \hat X\left( {k + 1|k} \right),P\left( {k + 1|k} \right) \to {{\hat X}^{\left( i \right)}}\left( {k + 1|k} \right),{w_m}^{\left( i \right)} \to {{\hat Y}^{\left( i \right)}}\left( {k + 1|k} \right),{w_m}^{\left( i \right)}\\ \hat Y\left( {k + 1|k} \right) = \sum {{w_m}^{\left( i \right)}{{\hat Y}^{\left( i \right)}}\left( {k + 1|k} \right)} \\ {P_{YY}} = \sum {{w_c}^{\left( i \right)}\left[ {{{\hat Y}^{\left( i \right)}}\left( {k + 1|k} \right) - \hat Y\left( {k + 1|k} \right)} \right]} {\left[ {{{\hat Y}^{\left( i \right)}}\left( {k + 1|k} \right) - \hat Y\left( {k + 1|k} \right)} \right]^T} + R\\ {P_{XY}} = \sum {{w_c}^{\left( i \right)}\left[ {{{\hat X}^{\left( i \right)}}\left( {k + 1|k} \right) - \hat X\left( {k + 1|k} \right)} \right]} {\left[ {{{\hat Y}^{\left( i \right)}}\left( {k + 1|k} \right) - \hat Y\left( {k + 1|k} \right)} \right]^T} \end{array} \right.}\\ {3融合增益:K\left( {k + 1} \right) = {P_{XY}}{P_{YY}}^{ - 1}}\\ {4后验估计:\hat X\left( {k + 1|k + 1} \right) = \hat X\left( {k + 1|k} \right) + K\left( {k + 1} \right)\left[ {Y\left( {k + 1} \right) - \hat Y\left( {k + 1|k} \right)} \right]}\\ {5后验方差:P\left( {k + 1|k + 1} \right) = P\left( {k + 1|k} \right) - K\left( {k + 1} \right){P_{YY}}{K^T}\left( {k + 1} \right)} \end{array}} \right.
⎩
⎨
⎧辅助步骤:X^(k∣k),P(k∣k)→X^(i)(k∣k),wm(i)→X^(i)(k+1∣k),wm(i)1先验估计:X^(k+1∣k)=∑wm(i)X^(i)(k+1∣k)2先验方差:P(k+1∣k)=∑wc(i)[X^(i)(k+1∣k)−X^(k+1∣k)][X^(i)(k+1∣k)−X^(k+1∣k)]T+Q辅助步骤:⎩
⎨
⎧X^(k+1∣k),P(k+1∣k)→X^(i)(k+1∣k),wm(i)→Y^(i)(k+1∣k),wm(i)Y^(k+1∣k)=∑wm(i)Y^(i)(k+1∣k)PYY=∑wc(i)[Y^(i)(k+1∣k)−Y^(k+1∣k)][Y^(i)(k+1∣k)−Y^(k+1∣k)]T+RPXY=∑wc(i)[X^(i)(k+1∣k)−X^(k+1∣k)][Y^(i)(k+1∣k)−Y^(k+1∣k)]T3融合增益:K(k+1)=PXYPYY−14后验估计:X^(k+1∣k+1)=X^(k+1∣k)+K(k+1)[Y(k+1)−Y^(k+1∣k)]5后验方差:P(k+1∣k+1)=P(k+1∣k)−K(k+1)PYYKT(k+1)
对于
n
n
n维状态向量，一般取
2
n
+
1
2n+1
2n+1个点对
(
X
∼
P
)
\left( {X \sim P} \right)
(X∼P)进行模拟，如下所示。具体的开根号分解方法有很多种，最为常用的方法为Cholesky分解法。
{
X
(
0
)
=
X
X
(
i
)
=
X
±
(
(
n
+
λ
)
P
)
1
,
2
,
.
.
.
,
n
,
i
=
1
,
2
,
.
.
.
,
2
n
\left\{ \begin{array}{l} {X^{\left( 0 \right)}} = X\\ {X^{\left( i \right)}} = X \pm {\left( {\sqrt {\left( {n + \lambda } \right)P} } \right)_{1,2,...,n}} \end{array} \right.,i = 1,2,...,2n
{X(0)=XX(i)=X±((n+λ)P)1,2,...,n,i=1,2,...,2n
对应的权重一般设计为如下形式：
{
w
m
(
0
)
=
λ
n
+
λ
w
m
(
i
)
=
1
2
(
n
+
λ
)
,
{
w
c
(
0
)
=
λ
n
+
λ
+
1
−
α
2
+
β
w
c
(
i
)
=
1
2
(
n
+
λ
)
,
i
=
1
,
2
,
.
.
.
,
2
n
\left\{ \begin{array}{l} {w_m}^{\left( 0 \right)} = \frac{\lambda }{{n + \lambda }}\\ {w_m}^{\left( i \right)} = \frac{1}{{2\left( {n + \lambda } \right)}} \end{array} \right.,\left\{ \begin{array}{l} {w_c}^{\left( 0 \right)} = \frac{\lambda }{{n + \lambda }} + 1 - {\alpha ^2} + \beta \\ {w_c}^{\left( i \right)} = \frac{1}{{2\left( {n + \lambda } \right)}} \end{array} \right.,i = 1,2,...,2n
{wm(0)=n+λλwm(i)=2(n+λ)1,{wc(0)=n+λλ+1−α2+βwc(i)=2(n+λ)1,i=1,2,...,2n
其中，具体参数
λ
,
α
,
β
,
κ
\lambda,\alpha,\beta,\kappa
λ,α,β,κ的设计一般满足如下设计规则：
{
λ
=
α
2
(
n
+
κ
)
−
n
α
∈
(
0
,
1
]
β
=
2
κ
≥
0
\left\{ \begin{array}{l} \lambda = {\alpha ^2}\left( {n + \kappa } \right) - n\\ \alpha \in \left( {0,1} \right]\\ \beta = 2\\ \kappa \ge 0 \end{array} \right.
⎩
⎨
⎧λ=α2(n+κ)−nα∈(0,1]β=2κ≥0
目前对上述参数的选择没有达成一致性结论。这里给一组具体数值仅供参考，当我们设计如下
λ
,
α
,
β
,
κ
\lambda,\alpha,\beta,\kappa
λ,α,β,κ时，可以得到：
{
λ
=
1
α
=
1
β
=
2
κ
=
1
→
{
X
(
0
)
=
X
X
(
i
)
=
X
±
(
(
n
+
1
)
P
)
1
,
2
,
.
.
.
,
n
,
{
w
m
(
0
)
=
1
n
+
1
w
m
(
i
)
=
1
2
(
n
+
1
)
,
{
w
c
(
0
)
=
1
n
+
1
+
2
w
c
(
i
)
=
1
2
(
n
+
1
)
,
i
=
1
,
2
,
.
.
.
,
2
n
\left\{ \begin{array}{l} \lambda = 1\\ \alpha = 1\\ \beta = 2\\ \kappa = 1 \end{array} \right. \to\left\{ \begin{array}{l} {X^{\left( 0 \right)}} = X\\ {X^{\left( i \right)}} = X \pm {\left( {\sqrt {\left( {n + 1} \right)P} } \right)_{1,2,...,n}} \end{array} \right., \left\{ \begin{array}{l} {w_m}^{\left( 0 \right)} = \frac{1}{{n + 1}}\\ {w_m}^{\left( i \right)} = \frac{1}{{2\left( {n + 1} \right)}} \end{array} \right.,\left\{ \begin{array}{l} {w_c}^{\left( 0 \right)} = \frac{1}{{n + 1}} + 2\\ {w_c}^{\left( i \right)} = \frac{1}{{2\left( {n + 1} \right)}} \end{array} \right.,i = 1,2,...,2n
⎩
⎨
⎧λ=1α=1β=2κ=1→{X(0)=XX(i)=X±((n+1)P)1,2,...,n,{wm(0)=n+11wm(i)=2(n+1)1,{wc(0)=n+11+2wc(i)=2(n+1)1,i=1,2,...,2n
2607
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
