卡尔曼平滑器

最新推荐文章于 2023-05-22 12:52:17 发布

HX71

最新推荐文章于 2023-05-22 12:52:17 发布

阅读量7.8k

点赞数 7

本文链接：https://blog.csdn.net/hxlove123456/article/details/89084791

版权

当看完邓老师的《信息融合滤波理论及其应用》之后，你就会发现信息融合的研究方向就是如下两个方面：1.系统状态的滤波、平滑、估计；2.系统噪声和观测噪声的估计。系统状态的滤波就是根据1,2,.....,t时刻的观测值估计当前t时刻的状态值；系统状态的平滑就是利用1,2,.....t,......t+k时刻的观测值估计当前t时刻的状态值；系统状态的预测就是利用1，2，........t时刻的观测值预测第t+1时刻的状态值。系统噪声的估计就是利用前t+N时刻的观测值，估计系统噪声w(t)的大小；观测噪声的估计就是利用前t+N时刻的观测值，估计观测噪声v(t)的大小，，示意图如下。

我的上一篇博客卡尔曼滤波的推导中，卡尔曼滤波的五个公式中就包含了状态的预测和估计过程，这篇博客主要讲系统状态的平滑过程。

卡尔曼滤波的平滑主要有三种平滑方式：固定点平滑、固定滞后平滑、固定区间平滑。这三种平滑方式的介绍我觉得严恭敏老师讲义上说的很清楚，所以直接贴上讲义上的描述：

对于具体卡尔曼平滑器的算法描述，我就不准备采用严老师讲义的算法了，因为严老师的算法虽然好理解，但是并不实用，存在很多矩阵求逆过程，计算过程过于复杂，所以这里介绍一下邓老师书中的平滑器算法。

定理1：对于如下系统：

$x\left ( t+1 \right )=\phi x\left ( t\right )+\Gamma w\left ( t \right )$ （1）

$y\left ( t \right )=Hx\left ( t\right )+v\left ( t \right )$ （2）

最优递推固定点Kalman平滑器为：

（3）

平滑误差有：

$P\left ( t/t+N \right )=P\left ( t/t+N-1 \right )-K\left ( t/t+N \right )\left [ HP\left ( t/t+N-1 \right )H^{T}+R \right ]K^{T}\left ( t/t+N \right )$ （4）

其中：

（5）

$\epsilon \left ( t+N \right )$ 为t+N时刻的新息，不懂的看上一篇博客卡尔曼滤波推导，里面有具体介绍。

证明过程：

由递推射影定理可知：

$\hat{x}\left ( t/t+N \right )=\hat{x}\left ( t/t+N-1 \right )+K\left ( t/t+N \right )\epsilon \left ( t+N \right )$ （6）

其中：

$K\left ( t/t+N \right )=E\left ( x\left ( t \right )*\epsilon ^{T}\left ( t+N \right )\right )*E\left ( \epsilon \left ( t+N \right )*\epsilon ^{T}\left ( t+N \right )\right )^{-1}$ （7）

因为有：

（8）

（9）

根据式9的一步误差递推式子，可以求出 $\epsilon \left ( t+N \right )$ 与 $\tilde{x}\left ( t/t-1 \right )$ 之间的关系表达式如下：

（10）

式9中， $\phi _{p}\left (t\right )=\phi \left ( I-KH\right )$ ，Ai，Bi分别为系数矩阵。

$x\left ( t \right )=\hat{x}\left ( t/t-1 \right )+\tilde{x}\left ( t/t-1 \right )$ （11）

因为存在：

$\hat{x}\left ( t/t-1 \right )\perp \tilde{x}\left ( t/t-1 \right )$

$\hat{x}\left ( t/t-1 \right )\perp w\left ( t+i \right )$ 当 $i\geq 0$ 时

$\hat{x}\left ( t/t-1 \right )\perp v\left ( t+i \right )$ 当 $i\geq 0$ 时

$\tilde{x}\left ( t/t-1 \right )\perp w\left ( t+i \right )$ 当 $i\geq 0$ 时

$\tilde{x}\left ( t/t-1 \right )\perp v\left ( t+i \right )$ 当 $i\geq 0$ 时

由式10和式11可得：

（12）

则有：

（13）

因为有：

（14）

（15）

定理得证。

定理2：最优固定滞后Kalman平滑器如下：

（16）

（17）

定理2可以通过定理1递推证明。

定理3：固定区间Kalman平滑器如下：

（18）

其中：

（19）

其中有 $r\left ( N+1/N \right )=0$ .

方差阵为：

（20）

其中有：

（21）

其中有： $S\left ( N+1/N \right )=0$

证明过程：

由定理2可知

当N=N-t时，带入上式有：

（22）

将K(t/t+i)的表达式带入上式可得：

（23）

则有：

（24）

（25）

则有：

对于方差：

（26）

（27）

由此有：

（28）

（29）

由此定理三得证。

综上所述，卡尔曼平滑器介绍完毕（这一篇文章用到了很多我上一篇讲到的知识点，有不懂的可以看看的上一篇博客卡尔曼滤波的推导）。

算法仿真：

考虑随机系统：

其中Qw=0.4,R=10,仿真效果图如下：

matlab仿真代码如下：

clc;
clear;
N=500;
A=[1,0;0.2,0.6];
G=[1;2];
H=[1,1];
Q=0.4;
R=10;
RandNum=normrnd(0,sqrt(Q),1,N);
RandNum1=normrnd(0,sqrt(R),1,N);
%%获得测量数据
x(:,1)=[0;0];
y(1)=0;
for i=2:N
    x(:,i)=A*x(:,i-1)+G*RandNum(i);
    y(i)=H*x(:,i)+RandNum1(i);
end
Pk(:,:,1)=[Q,0;0,R];
X(:,1)=[0;0];
I=eye(2);
K(:,1)=[0;0];
Xk(:,1)=[0;0];
Pkk(:,:,1)=[0,0;0,0];
for i=2:N
    Xk(:,i)=A*X(:,i-1);
    Pkk(:,:,i)=A*Pk(:,:,i-1)*A'+G*Q*G';
    K(:,i)=Pkk(:,:,i)*H'/(H*Pkk(:,:,i)*H'+R);
    X(:,i)=Xk(:,i)+K(:,i)*(y(i)-H*Xk(:,i));
    Pk(:,:,i)=(I-K(:,i)*H)*Pkk(:,:,i);
end
N1=3;
for i=2:N-N1-1
    Ks=K(:,i)*(y(i)-H*Xk(:,i));
    for j=2:N1+1
            KN=eye(2);
            for k=1:j-1
                 KN=KN*(I-K(:,i+k-1)*H)'*A';
            end
            KN=Pkk(i)*KN*H'/(H*Pkk(i+j-1)*H'+R);
            Ks=Ks+KN*(y(i+j-1)-H*Xk(:,i+j-1));
    end
    X2(:,i)=Xk(:,i)+Ks;
end 
X2(:,N-2)=X(:,N-2);
X2(:,N-1)=X(:,N-1);
X2(:,N)=X(:,N);
t=1:N;
figure(1);
subplot(2,2,1);
plot(t,x(1,:),'r',t,X(1,:),'k');
subplot(2,2,2);
plot(t,x(2,:),'r',t,X(2,:),'k');
subplot(2,2,3);
plot(t,X(1,:),'r',t,X2(1,:),'k');
subplot(2,2,4);
plot(t,X(2,:),'r',t,X2(2,:),'k');
figure(2);
plot(t,X(1,:)-x(1,:),'r',t,X2(1,:)-x(1,:),'k');
figure(3);
plot(t,X(2,:)-x(2,:),'r',t,X2(2,:)-x(2,:),'k');

HX71

关注

7
点赞
踩
56

收藏

觉得还不错? 一键收藏
1
评论
卡尔曼平滑器

当看完邓老师的《信息融合滤波理论及其应用》之后，你就会发现信息融合的研究方向就是如下两个方面：1.系统状态的滤波、平滑、估计；2.系统噪声和观测噪声的估计。系统状态的滤波就是根据1,2,.....,t时刻的观测值估计当前t时刻的状态值；系统状态的平滑就是利用1,2,.....t,......t+k时刻的观测值估计当前t时刻的状态值；系统状态的预测就是利用1，2，........t时刻的观测值预测第...
复制链接

扫一扫