[Matlab]卡尔曼滤波器设计

[Matlab]卡尔曼滤波器设计

1、卡尔曼滤波器

​ 卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

​ 卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑便使用了这种滤波器。关于这种滤波器的论文最早由Swerling(1958)发表了这种想法。

​ 数据滤波是去除噪声还原真实数据的一种数据处理技术，Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。由于它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，Kalman滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。

2、卡尔曼滤波器性质

①卡尔曼滤波是一个算法，它适用于线性、离散和有限维系统。每一个有外部变量的自回归移动平均系统(ARMAX)或可用有理传递函数表示的系统都可以转换成用状态空间表示的系统，从而能用卡尔曼滤波进行计算。

②任何一组观测数据都无助于消除x(t)的确定性。增益K(t)也同样地与观测数据无关。

③当观测数据和状态联合服从高斯分布时用卡尔曼递归公式计算得到的是高斯随机变量的条件均值和条件方差，从而卡尔曼滤波公式给出了计算状态的条件概率密度的更新过程线性最小方差估计，也就是最小方差估计。

3、基本动态模型

前提条件：

假设1：k时刻的真实状态是从$k-1$时刻的真实状态演化而来；
假设2：演化与测量的过程由线性算子来描述。

3.1两个基本的方程

四个状态值的定义：
${\hat{x}}_{k-1|k-1}$ :表示$k-1$ 时刻的估计状态
$x_{k-1}$ : 表示$k-1$时刻的真实状态
${\hat{x}}_{k|k-1}$ :表示$k-1$时刻的估计状态预测k时刻估计的预测值
${\hat{x}}_{k|k}$ :表示$k$时刻的估计状态
$x_k$ : 表示$k$时刻的真实状态
状态转移方程：
$x_k=F_k{x}_{k-1}+B_k{u}_k+w_k$

• $F_k$:作用在$k-1$时刻状态$x_{k-1}$上的变换矩阵；
• $B_k$:作用在控制器向量$u_k$上的控制矩阵；
• $w_k$:过程噪声，并假定；均值为零，协方差矩阵为$Q_k$的独立多元正态分布，记作：$w_{k}N(0,Q_k).Q_k=cov(w_k)$

状态测量方程：

​ $z_k=H_k{x}_k+v_k$ $(2{)}^{\ast }$

• $H_k$:观测矩阵，作用是将隐含的真实状态空间映射到观测空间；
• $v_k$:观测噪声，并假定；均值为零，协方差矩阵为$R_k$的独立多元正态分布，记作：$v_{k}N(0,R_k).R_k=cov(v_k)$

基于$k-1$时刻状态对$k$时刻状态的估计值与真实值之间的差称为估计误差，该估计误差的协方差矩阵的定义为后验估计误差协方差矩阵，用下式表示：

$P_{k|k-1}=cov(x_k-{\hat{x}}_{k|k-1})$ $(3{)}^{\ast }$

其模型拓扑结构用隐马尔科夫链可以表示成图1（维基百科:卡尔曼滤波）这样：

​ 图1 卡尔曼滤波的隐马尔可夫链式模型

3.2两个基本的过程

(1)预测过程

第一步：$k-1$时刻的估计状态预测k时刻估计的预测值

${\hat{x}}_{k|k-1}=F_k{\hat{x}}_{k-1|k-1}+B_k{u}_k$ (1)

注意：(1)与$(1{)}^{\ast }$的差别，预测是理想值，不能引入不可控的随机噪音。

第二步：计算后验估计误差协方差矩阵用来度量预测精度

$P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+Q_k$ (2)

公式推导：

$P_{k|k-1}=cov(F_k{x}_{k-1}+B_k{u}_k+w_k-F_k{\hat{x}}_{k-1|k-1}-B_k{u}_k)$

​ $=cov(F_k(x_{k-1}-{\hat{x}}_{k-1|k-1})+w_k)$

​ $=F_{k}cov((x_{k-1}-{\hat{x}}_{k-1|k-1})F_k^T+COV(w_k))$

​ $=F_k{P}_{k-1|k-1}{F}_k^T+Q_k$

(2) 更新过程

​ 我们基于$k-1$时刻对k时刻状态的估计是否正确，需要用与实际测量值之间的误差来衡量，并且考虑用这个误差来补偿。所以在更新之前，我们应该计 算实际测量值与估计输出值之间的差值及其协方差矩阵。

​ ${\tilde{y}}_k=z_k+H_k{\hat{x}}_{k|k-1}$ (3)

注意：此处代入方程与$(2{)}^{\ast }$式的区别。

（3）式的协方差矩阵表示为：

​ $S_k=H_k{P}_{k|k-1}{H}_k^T+R_k$ (4)

下面推导公式（4）：

$S_k=cov({\tilde{y}}_k)=cov(z_k+H_k{\hat{x}}_{k|k-1})=cov(H_k{x}_k+v_k+H_k{\hat{x}}_{k|k-1})$

​ $=cov(H_k(x_k-{\hat{x}}_{k|k-1}))+cov(v_k)=H_{k}cov((x_k-{\hat{x}}_{k|k-1})H_k^T+R_k)$

​ $=H_k{P}_{k|k-1}{H}_k^T+R_k$

然后我们再进行更新的步骤。更新是指：由基于k-1时刻对k时刻状态的估计值应当如何得到k时刻的估计值。卡尔曼的思想就是：用基于k-1时刻对k时刻状态的估计值与预测输出值和实际输出值之间的差进行线性组合得到k时刻的估计值，连接这两者的就是卡尔曼增益。这里体现的就是反馈的思想，更新过程的第一步用下式表示：

第一步：$k$时刻的估计状态${\hat{x}}_{k|k}$由${\hat{x}}_{k|k-1}$和${\tilde{y}}_k$线性组合而来：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k{\tilde{y}}_k$ (5)

其中：$K_k$为卡尔曼增益。

那么现在问题来了，如何求取这个卡尔曼增益呢？
这时候我们应该回到我们的出发点，我们希望的是滤除干扰真实状态的噪声，是滤波器的估计状态与真实状态最为接近。最为接近可以理解为k时刻的真实状态与k时刻的估计状态之间的误差二范平方和最小，也就等价于协方差矩阵的迹最小。可以表示为：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-z7rOsR1s-1575739458354)(G:\研究生\项目小组任务\笔记\第四周和第五周笔记\KALMAN.png)]

在这种情况下求取的卡尔曼增益称为最优卡尔曼增益。求取的过程就是直接上式对卡尔曼增益求一阶导数。

第二步，计算卡尔曼增益：

$K_k=P_{k|k-1}{H}_k^T{S}_k^{-1}$ (6)

下面给出推导过程：

最优卡尔曼增益计算出来之后，我们发现在最优卡尔曼增益情况下可以对后验误差协方差矩阵进行简化。第三步就是计算在最优卡尔曼增益下的后验误差协方差矩阵：

第三步:计算在最优卡尔曼增益$K_k$下简化后验预测误差协方差矩阵：

$P_{k|k}=(I-K_k{H}_k)P_{k|k-1}$ (7)

推导过程：

$P_{k|k}=P_{k|k-1}-K_k{H}_k{P}_{k|k-1}-P_{k|k-1}{H}_k^T{K}_k^T+K_k^T{S}_k{K}_k^T$

​ $=(I-K_k{H}_k)P_{k|k-1}-P_{k|k-1}{H}_k^T{K}_k^T+P_{k|k-1}{H}_k^T{K}_k^T$

​ $=(I-K_k{H}_k)P_{k|k-1}$

4、综上所述:

通过上面的尔曼在滤波器的推导过程分析可得：

(1)预测过程

第一步： ${\hat{x}}_{k|k-1}=F_k{\hat{x}}_{k-1|k-1}+B_k{u}_k$ (1)

第二步：$P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+Q_k$ (2)

​ ${\tilde{y}}_k=z_k+H_k{\hat{x}}_{k|k-1}$ (3)

​ $S_k=H_k{P}_{k|k-1}{H}_k^T+R_k$ (4)

(2) 更新过程

第一步：${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k{\tilde{y}}_k$ (5)

第二步：$K_k=P_{k|k-1}{H}_k^T{S}_k^{-1}$ (6)

第三步：$P_{k|k}=(I-K_k{H}_k)P_{k|k-1}$ (7)

将这些方程做成框图：

从中可以看到，只要对初始状态进行设定，卡尔曼滤波器就可以完成迭代了。

5、卡尔曼滤波器程序设计：

(1).温度测量

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
% 程序说明：Kalman滤波用于温度测量的实例(一维）    
function main  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%初始化参数
N=120;  %采样点个数
A=1;  
B=1;  
H=1; 
Q=0.01;  
R=0.25;  
W=sqrt(Q)*randn(1,N);  
V=sqrt(R)*randn(1,N);  
CON=25;  %温度真实值
%分配空间
X=zeros(1,N);    
Z=zeros(1,N); 
Xkf=zeros(1,N);  
P=zeros(1,N);   
X(1)=25.1;  
P(1)=0.01;  
Z(1)=24.9;  
Xkf(1)=Z(1);  
I=eye(1);   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%kalman过程
for k=2:N  
    X(k)=A*X(k-1)+B*W(k);   
    Z(k)=H*X(k)+V(k);  
    X_pre(k)=A*Xkf(k-1);             
    P_pre(k)=A*P(k-1)*A'+Q;          
    Kg=P_pre(k)/(H*P_pre(k)*H'+R);             
    Xkf(k)=X_pre(k)+Kg*(Z(k)-H*X_pre(k));           
    P(k)=(I-Kg*H)*P_pre(k);  
end  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%误差过程
Err_Messure=zeros(1,N);  
Err_Kalman=zeros(1,N);  
for k=1:N  
    Err_Messure(k)=Z(k)-X(k);  
    Err_Kalman(k)=Xkf(k)-X(k);  
end  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t=1:N;  
figure;  
plot(t,CON*ones(1,N),'-b',t,X,'-r',t,Z,'g+',t,Xkf,'-k');  
legend('期望值','真实值','测量值','kalman估计值');           
xlabel('采样时间');  
ylabel('温度');  
title('Kalman Filter Simulation');  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;  
plot(t,Err_Messure,'-b',t,Err_Kalman,'-k');  
legend('测量误差','kalman误差');           
xlabel('采样时间');  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

(2).自由落体追踪

%kalman滤波用于自由落体运动目标追踪（二维，x,v)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%初始化参数
N=1000;
Q=[0,0;0,0];
R=0.5;
w=sqrt(Q)*randn(2,N);
v=sqrt(R)*randn(1,N);
A=[1,1;0,1];%状态转移矩阵
B=[0.5;1];%控制矩阵
U=-10;%控制变量
H=[1,0];%观测矩阵
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%分配空间，x,p0,z,xkf
x=zeros(2,N);%物体真实状态
x(:,1)=[95;1];%初始位移和速度
p0=[10,0;0,1];%初始误差
z=zeros(1,N);%观测值
z(1)=H*x(:,1);%观测真实值,第一列的列向量，
xkf=zeros(2,N);%kalman估计状态
xkf(:,1)=x(:,1);%kalman估计状态初始化,第一列的列向量，
err_p=zeros(N,2);
err_p(1,1)=p0(1,1);
err_p(1,2)=p0(2,2);
I=eye(2);%2*2单位矩阵表明二维系统
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%kalman迭代过程
for k=2:N;
    x(:,k)=A*x(:,k-1)+B*U+w(k);%模型
    z(k)=H*x(:,k)+v(k);%模型
    x_pre=A*xkf(:,k-1)+B*U;%①
    p_pre=A*p0*A'+Q;%②
    kg=p_pre*H'/(H*p_pre*H'+R);%③
    xkf(:,k)=x_pre+kg*(z(k)-H*x_pre);%④
    p0=(I-kg*H)*p_pre;%⑤
    err_p(k,1)=p0(1,1);%位移误差均方值
    err_p(k,2)=p0(2,2);%速度误差均方值
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%误差计算
messure_err_x=zeros(1,N);
kalman_err_x=zeros(1,N);
kalman_err_v=zeros(1,N);
for k=1:N
    messure_err_x(k)=z(k)-x(1,k);%位移的测量误差
    kalman_err_x(k)=xkf(1,k)-x(1,k);%kalman估计位移与真实位移之间偏差
    kalman_err_v(k)=xkf(2,k)-x(2,k);%kalman估计速度与真实速度之间偏差
end
%噪声图
figure;
plot(w);xlabel('采样时间');ylabel('过程噪声');
figure;
plot(v);xlabel('采样时间');ylabel('测量噪声');
%位置误差
figure;
hold on,box on;
plot(messure_err_x,'-r.');
plot(kalman_err_x,'-g.');
legend('测量位置','kalman估计位置');
xlabel('采样时间');ylabel('位置偏差');
%kalman速度偏差
figure;
plot(kalman_err_v);
xlabel('采样时间');ylabel('速度偏差');
%均方值
figure;
plot(err_p(:,1));
xlabel('采样时间');ylabel('位移误差均方值');
figure;
plot(err_p(:,1));
xlabel('采样时间');ylabel('速度误差均方值');

(3).GPS导航定位

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
%  Kalman滤波在船舶GPS导航定位系统中的应用(四维，x方向x,v   ,y方向x,v; ） 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
function main  
T=1;  %采样周期
N=80/T;  %总的采样次数
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];  
H=[1,0,0,0;0,0,1,0];  
Q=4e-4  ;
R=100*eye(2);
X=zeros(4,N);  
X(:,1)=[100,2,200,20]; %x四维，初始位置（-100,200）水平速度2，垂直速度20 
Z=zeros(2,N);  
Z(:,1)=[X(1,1),X(3,1)];  %z只观测到位置，没有速度
Xkf=zeros(4,N);  
Xkf(:,1)=X(:,1);  
P0=eye(4);  %协方差矩阵初始化，4*4单位矩阵
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
for t=2:N  %模型
    X(:,t)=F*X(:,t-1)+sqrt(Q)*randn(4,1);%目标真实轨迹  randn(4,1)4*1随机矩阵
    Z(:,t)=H*X(:,t)+sqrt(R)*randn(2,1); %观测轨迹randn(2,1)2*1随机矩阵
end  
for i=2:N  
    X_pre=F*Xkf(:,i-1);  
    P_pre=F*P0*F'+Q;  
    K=P_pre*H'/(H*P_pre*H'+R);  
    Xkf(:,i)=X_pre+K*(Z(:,i)-H*X_pre);  
    P0=(eye(4)-K*H)*P_pre;  
end  
for i=1:N  
    Err_Observation(i)=RMS(X(:,i),Z(:,i));  
    Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));  
end  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;  
hold on;box on;  
plot(X(1,:),X(3,:),'-k');  %位置轨迹
plot(Z(1,:),Z(2,:),'-b.');  %观测轨迹
plot(Xkf(1,:),Xkf(3,:),'-r+');  %Kalman估计轨迹
legend('真实轨迹','观测轨迹','滤波轨迹')  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;  
hold on; box on;  
plot(Err_Observation,'-ko','MarkerFace','g')  
plot(Err_KalmanFilter,'-ks','MarkerFace','r')  
legend('滤波前误差','滤波后误差')  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
function dist=RMS(X1,X2);  
if length(X2)<=2  
    dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(2))^2 );  
else  
    dist=sqrt( (X1(1)-X2(1))^2 + (X1(3)-X2(3))^2 );  
end  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%