一、两个方程
1,系统的状态方程(预测方程)
x
k
=
A
k
x
k
−
1
+
B
k
u
k
+
w
k
x_k = A_kx_{k-1} + B_ku_k + w_k
xk=Akxk−1+Bkuk+wk
2,测量方程
z
k
=
H
k
x
k
+
v
k
z_k = H_kx_k+v_k
zk=Hkxk+vk
其中:
x k x_k xk是状态向量;
A k A_k Ak是状态转移矩阵;
u k u_k uk是状态控制向量;
B k B_k Bk是控制变量矩阵;
w k w_k wk是控制系统的噪声,服从高斯分布: w k ∼ N ( 0 , Q ) w_k\sim N(0,Q) wk∼N(0,Q)
Q Q Q是系统噪声协方差矩阵;
z k z_k zk是测量向量;
H k H_k Hk是状态向量到测量向量的转换矩阵;
v k v_k vk是测量的噪声,服从高斯分布: v k ∼ N ( 0 , R ) v_k\sim N(0,R) vk∼N(0,R)
R R R是测量噪声的协方差矩阵。
二、五个公式
预测:
x
^
k
ˉ
=
A
k
x
^
k
−
1
+
B
k
u
k
P
k
ˉ
=
A
k
P
k
−
1
A
k
T
+
Q
\hat x_{\bar k}=A_k\hat x_{k-1}+B_ku_k \\ P_{\bar k}=A_kP_{k-1}A^T_k+Q
x^kˉ=Akx^k−1+BkukPkˉ=AkPk−1AkT+Q
更新:
K
k
=
P
k
ˉ
H
k
T
H
k
P
k
ˉ
H
k
T
+
R
x
^
k
=
x
^
k
ˉ
+
K
k
(
z
k
−
H
k
x
^
k
ˉ
)
P
k
=
(
I
−
K
k
H
k
)
P
k
ˉ
K_k= {P_{\bar k}H^T_k\over H_kP_{\bar k}H^T_k+R} \\ \hat x_k =\hat x_{\bar k}+K_k(z_k-H_k\hat x_{\bar k}) \\ P_k = (I-K_kH_k)P_{\bar k}
Kk=HkPkˉHkT+RPkˉHkTx^k=x^kˉ+Kk(zk−Hkx^kˉ)Pk=(I−KkHk)Pkˉ
其中:
- x ^ k − 1 \hat x_{k-1} x^k−1和 x ^ k \hat x_k x^k,分别为k-1时刻与k时刻的后验状态估计,是滤波结果之一,即更新后的结果,也叫最优估计。(因为不可能知道真实值,所以加一个hat,表示最优估计)
- x ^ k ˉ \hat x_{\bar k} x^kˉ,表示k时刻的的先验状态估计,及根据k-1时刻的最优估计预测k时刻的状态。
- P k − 1 P_{k-1} Pk−1和 P k P_k Pk,分别为k-1时刻与k时刻的后验估计协方差(即 x ^ k − 1 \hat x_{k-1} x^k−1和 x ^ k \hat x_k x^k的协方差,表示状态的不确定性),为滤波结果之一。
- P k ˉ P_{\bar k} Pkˉ,为k时刻的先验估计协方差( x ^ k ˉ \hat x_{\bar k} x^kˉ的协方差)。
- K k K_k Kk,为卡尔曼增益。
三、五个公式的推导过程
1,预测
定理1:随机变量函数的分布(高斯分布)
已知 X X X服从高斯分布: X ∼ N ( μ , σ 2 ) X\sim N(\mu,\sigma^2) X∼N(μ,σ2),且 Y = a X + b Y=aX+b Y=aX+b。则 Y Y Y服从高斯分布: Y ∼ N ( a μ + b , a 2 σ 2 ) Y\sim N(a\mu+b,a^2\sigma^2) Y∼N(aμ+b,a2σ2)
已知k-1时刻系统的最优状态服从高斯分布: X ^ k − 1 ∼ N ( x ^ k − 1 , P k − 1 ) \hat X_{k-1}\sim N(\hat x_{k-1},P_{k-1}) X^k−1∼N(x^k−1,Pk−1)
根据预测方程: x k = A k x k − 1 + B k u k + w k x_k = A_kx_{k-1} + B_ku_k + w_k xk=Akxk−1+Bkuk+wk
及上面的定理,可以直接推导出k时刻的预测状态,服从高斯分布:
X ^ k ˉ ∼ N ( A k x ^ k − 1 + B k u k , A k P k − 1 A k T + Q ) \hat X_{\bar k}\sim N(A_k\hat x_{k-1} + B_ku_k,A_kP_{k-1}A^T_k+Q) X^kˉ∼N(Akx^k−1+Bkuk,AkPk−1AkT+Q)
从而得到k时刻的预测状态:
x
^
k
ˉ
=
A
k
x
^
k
−
1
+
B
k
u
k
P
k
ˉ
=
A
k
P
k
−
1
A
k
T
+
Q
\hat x_{\bar k}=A_k\hat x_{k-1}+B_ku_k \\ P_{\bar k}=A_kP_{k-1}A^T_k+Q
x^kˉ=Akx^k−1+BkukPkˉ=AkPk−1AkT+Q
PS1:因为是根据k-1时刻预测得到的k时刻的状态,因此在变量上加了个-,表示k时刻的先验估计。
PS2:这里的变量及参数都是矩阵的形式,因此k-1时刻的协方差矩阵 P k − 1 P_{k-1} Pk−1的变换形式为 A k P k − 1 A k T A_kP_{k-1}A^T_k AkPk−1AkT。
2,更新
定理2:两个高斯分布的乘积依然为高斯分布
已知 X X X服从高斯分布: X ∼ N ( μ 1 , σ 1 2 ) X\sim N(\mu_1,\sigma_1^2) X∼N(μ1,σ12), Y Y Y服从高斯分布: Y ∼ N ( μ 2 , σ 2 2 ) Y\sim N(\mu_2,\sigma_2^2) Y∼N(μ2,σ22),则 X Y XY XY服从高斯分布: X Y ∼ N ( μ , σ 2 ) XY\sim N(\mu,\sigma^2) XY∼N(μ,σ2)
K = σ 1 2 σ 1 2 + σ 2 2 μ = μ 1 + K ( μ 2 − μ 1 ) σ 2 = ( 1 − K ) σ 1 2 K={\sigma_1^2\over \sigma_1^2+\sigma_2^2} \\ \mu =\mu_1+K(\mu_2-\mu_1) \\ \sigma^2 = (1-K)\sigma_1^2 K=σ12+σ22σ12μ=μ1+K(μ2−μ1)σ2=(1−K)σ12
由上面知道k时刻的预测状态服从高斯分布, X ^ k ˉ ∼ N ( x ^ k ˉ , P k ˉ ) \hat X_{\bar k}\sim N(\hat x_{\bar k},P_{\bar k}) X^kˉ∼N(x^kˉ,Pkˉ)
且由测量方程: z k = H k x k + v k z_k = H_kx_k+v_k zk=Hkxk+vk
按照定理1进行转换,即将k时刻的预测状态转换到k时刻的测量状态,同样服从高斯分布: Z ^ k ˉ ∼ N ( H k x ^ k ˉ , H k P k ˉ H k T ) \hat Z_{\bar k}\sim N(H_k\hat x_{\bar k},H_kP_{\bar k}H^T_k) Z^kˉ∼N(Hkx^kˉ,HkPkˉHkT)。
注意:这里协方差矩阵 H k P k ˉ H k T H_kP_{\bar k}H^T_k HkPkˉHkT没有 + R +R +R是因为这里只是状态的转换,并不包含实际的测量。
测量本身服从高斯分布: Z k ∼ N ( z k , R ) Z_k\sim N(z_k,R) Zk∼N(zk,R)
我们对上面两个高斯分布进行相乘,即k时刻预测的乘以k时刻测量的,便得到k时刻状态的最优估计:
X ^ k ∼ N ( x ^ k , P k ) \hat X_k\sim N(\hat x_k,P_k) X^k∼N(x^k,Pk)
这里需要注意的是,我们同样需要把k时刻状态的最优估计转换到测量状态下: Z ^ k ∼ N ( H k x ^ k , H k P k H k T ) \hat Z_k\sim N(H_k\hat x_k,H_kP_kH^T_k) Z^k∼N(Hkx^k,HkPkHkT)。
按照定理2:
Z
^
k
ˉ
×
Z
k
=
Z
^
k
\hat Z_{\bar k}\times Z_k = \hat Z_k
Z^kˉ×Zk=Z^k
K
′
=
H
k
P
k
ˉ
H
k
T
H
k
P
k
ˉ
H
k
T
+
R
H
k
x
^
k
=
H
k
x
^
k
ˉ
+
K
′
(
z
k
−
H
k
x
^
k
ˉ
)
H
k
P
k
H
k
T
=
(
I
−
K
′
)
H
k
P
k
ˉ
H
k
T
K' = {H_kP_{\bar k}H^T_k\over H_kP_{\bar k}H^T_k+R} \\ H_k\hat x_k = H_k\hat x_{\bar k} + K'(z_k-H_k\hat x_{\bar k}) \\ H_kP_kH^T_k = (I-K')H_kP_{\bar k}H^T_k
K′=HkPkˉHkT+RHkPkˉHkTHkx^k=Hkx^kˉ+K′(zk−Hkx^kˉ)HkPkHkT=(I−K′)HkPkˉHkT
其中:
K
=
K
′
H
k
=
P
k
ˉ
H
k
T
H
k
P
k
ˉ
H
k
T
+
R
K = {K'\over H_k}={P_{\bar k}H^T_k\over H_kP_{\bar k}H^T_k+R}
K=HkK′=HkPkˉHkT+RPkˉHkT
将上式中的
K
K
K替换后,即可得到更新的三个公式:
K
k
=
P
k
ˉ
H
k
T
H
k
P
k
ˉ
H
k
T
+
R
x
^
k
=
x
^
k
ˉ
+
K
k
(
z
k
−
H
k
x
^
k
ˉ
)
P
k
=
(
I
−
K
k
H
k
)
P
k
ˉ
K_k= {P_{\bar k}H^T_k\over H_kP_{\bar k}H^T_k+R} \\ \hat x_k =\hat x_{\bar k}+K_k(z_k-H_k\hat x_{\bar k}) \\ P_k = (I-K_kH_k)P_{\bar k}
Kk=HkPkˉHkT+RPkˉHkTx^k=x^kˉ+Kk(zk−Hkx^kˉ)Pk=(I−KkHk)Pkˉ
至此,便得到了卡尔曼滤波的结果:
x
^
k
\hat x_k
x^k和
P
k
P_k
Pk
PS3:这里需要注意的一点是要将状态方程全部转换到测量状态下才可以进行高斯相乘。否则的话,他们的矩阵都不匹配。
四、参数矩阵
上面的公式中的变量及参数都是矩阵的形式,并且矩阵的行列并不一致。这里举例说明下。
假设状态向量
x
x
x为
x
n
×
1
x_{n\times 1}
xn×1,测量向量
z
z
z为
z
m
×
1
z_{m\times 1}
zm×1。控制变量
u
u
u为
u
a
×
1
u_{a\times 1}
ua×1。
{
A
n
×
n
B
n
×
a
P
n
×
n
Q
n
×
n
H
m
×
n
K
n
×
m
R
m
×
m
I
n
×
n
\begin{cases} A_{n\times n} \\ B_{n\times a} \\ P_{n\times n} \\ Q_{n\times n} \\ H_{m\times n} \\ K_{n\times m} \\ R_{m\times m} \\ I_{n\times n} \\ \end{cases}
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧An×nBn×aPn×nQn×nHm×nKn×mRm×mIn×n