卡尔曼滤波及Matlab实现

卡尔曼滤波及Matlab实现

一、什么是卡尔曼滤波

 滤波是从信号中提取有用信息的过程，比如从电信号中提取有用的频谱分量，从观测到的物体轨迹中提取位置信息，滤除图像信号中的噪声等。卡尔曼滤波是一种有效的滤波方法。如果已知一个系统的状态方程，又可以通过外部手段对系统进行观测，得到量测方程，就可以应用卡尔曼滤波估计系统的状态。

1.1卡尔曼滤波的目标和使用条件

 卡尔曼滤波是一种线性最小方差估计，目的是使估计的均方误差最小，即：
$$E\{[X-\hat{X}(Z)][X-\hat{X}(Z){]}^T\}$$

最小。$\hat{X}(Z)$为用Z计算而得的X的最小方差估计值，且$\hat{X}(Z)$为量测向量Z的线性函数，即：
$$\hat{X}=AZ+b$$
$\tilde{X}=X-\hat{X}$为估计误差，又称残差。
这里估计的$X$是一个观测值，也就是实际情况中得到的数据，在组合导航中就是物体真实的运动参数。
线性最小方差估计 $\hat{X}$具有以下性质：
无偏性，即$E\{\hat{X}\}=E\{X\}$。
正交性，即$E\{[(X-\hat{X})Z^T]\}=0$。
$E\{\tilde{X}\cdot {\tilde{X}}^T\}$ 为估计均方误差阵。由最无偏性可得：
$$E\{[X-\hat{X}]\}=E\{\tilde{X}\}=0$$
估计的均方误差就是估计误差的方差，即：
$E\{\tilde{X}\cdot {\tilde{X}}^T\}=E\{[\tilde{X}-E(\tilde{X})]\cdot [\tilde{X}-E(\tilde{X}){]}^T\}$
由上式可以看出，最小方差估计不但使估值X ̂的均方误差最小，且这种最小均方误差就是X ̂的误差的方差。
 卡尔曼滤波适用于线性系统。要求系统的状态方程和量测方程线性。，即
&X&
系统的状态方程：$X_k={\Phi }_{(k⁄(k-1))}{X}_{(k-1)}+W_k$
系统的量测方程：$Z_k=H{X}_k+V_k$
  上面两个公式中，$X_k$是k时刻的系统状态，$Z_k$是$k$时刻的量测值，H是量测系统的参数，$W_k$和$V_k$分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的协方差(covariance)分别是$Q$，$R$（这里我们假设他们不随系统状态变化而变化）。
 计算机处理离散系统，但是现实世界中的系统大多是连续的。为了在计算机上计算，我们需要将连续系统转化为离散系统。在很多应用场景中，系统状态方程是微分方程（比如惯性导航系统），状态转移矩阵${\Phi }_{(k⁄(k-1))}$可以通过求解微分方程得出。具体的解法在信号与系统教材中可以查到。总之，我们可以将微分方程变成离散的递推方程，以便编程。
总结一下，卡尔曼滤波的适用条件为：
1、状态方程和量测方程线性
2、系统噪声和量测噪声为白噪声
3、$Q$和$R$为定值，不随时间变化。

1.2 卡尔曼滤波公式

 卡尔曼滤波的流程如上图。
①式为状态一步预测方程，通过上个时刻的估计值预测出一个此时刻的一步预测值X ̂_(k⁄(k-1))。
②式是一步预测均方误差方程,P_(k∕(k−1))是X ̂_(k⁄(k-1))的方差。
③式计算滤波增益K_k。可以直观的看出量测噪声协方差R越大，K_k越小。K_k反映出一步预测值和量测值在估计值中的占比。
④式是状态估计方程，由两部分组成：一步预测值和新息过程乘以滤波增益。我们把Z_k−H_k (X ̂_(k∕[k−1]))称为新息过程（innovation），可以理解成Z_k为系统注入了新的信息。
⑤式计算估计均方误差，即P_k是状态估计值X ̂_k的方差。同时，根据卡尔曼滤波的定义，P_k也是评判卡尔曼滤波效果的标准。滤波趋于稳定之后，P_k 会稳定在一个固定的较小值附近。P_k越小，说明均方误差越小，卡尔曼滤波效果越好。

在以上公式中还有一些需要注意的地方：
1、对于有些系统，Φ_(k⁄(k-1)) 是时不变的，比如匀速直线运动系统。但是对于有些系统，比如惯性导航系统，Φ_(k⁄(k-1)) 时变，Φ_(k⁄(k-1)) 与系统中状态变量的输入量有关。
2、如果Φ_(k⁄(k-1)) 时不变，P_k只与先验信息Q和R有关。也就是说，P_k完全由先验信息决定，与量测值无关。如果建模不准确，P_k可能不能准确反映X_k的真实情况，会导致滤波发散。
3、Φ_(k⁄(k-1)) 时变的情况稍后分析。

1.3 卡尔曼滤波和INS/GPS组合导航

INS/GPS组合导航系统的公式太复杂，这里就不写了。INS/GPS组合导航系统的状态方程是惯导的误差方程。我们需要根据先验信息估计惯导的误差，解算出的经纬度等信息用来更新Φ_(k⁄(k-1)) ，这里Φ_(k⁄(k-1)) 是时变的。注意，我们需要根据先验信息确定一部分F矩阵的值，F矩阵的另一部分和姿态矩阵，速度、经纬度有关，姿态矩阵、速度和经纬度由惯导传感器量测值经解算、滤波后得出。F矩阵积分得到Φ_(k⁄(k-1)) 。如果惯导传感器的测量值误差太大，即实际的Q值偏离建模时使用的根据先验估计得到的Q值太多，会导致滤波发散。

1.4 卡尔曼滤波的拓展

1.1节中阐述了卡尔曼滤波的适用条件。研究者在标准卡尔曼滤波的基础上做了一些拓展，是卡尔曼滤波可以适用于更宽松的条件。如果系统非线性，可以使用扩展卡尔曼滤波或者无迹卡尔曼滤波。如果R和Q不满足白噪声要求，即R和Q时变，可以采用自适应卡尔曼滤波。

二、Matlab程序示例

2.1 匀速直线运动

function main
clc;clear;
T=1;%雷达扫描周期
N=100/T;%总的采样次数
X=zeros(4,N);%目标真实位置、速度
X(:,1)=[-100,2,200,20];%目标初始位置、速度
S(:,1)=[-100,2,200,20];
Z=zeros(2,N);%传感器对位置的观测
Z(:,1)=[X(1,1),X(3,1)];%观测初始化
delta_w=1e-2; %如果增大这个参数，目标真实轨迹就是曲线了
Q=delta_w*diag([0.5,1,0.5,1]);%过程噪声均值
R=eye(2);%观测噪声均值
phi=[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];%观测矩阵
Xn=zeros(4,0);
for i=2:N
    S(:,i)=phi*S(:,i-1);%目标理论轨迹
    X(:,i)=phi*X(:,i-1)+sqrtm(Q)*randn(4,1);%目标真实轨迹
    Z(:,i)=H*X(:,i)+sqrtm(R)*randn(2,1);%对目标的观测
end

% Kalman 滤波
Xkf=zeros(4,N);
Xkf(:,1)=X(:,1);%卡尔曼滤波状态初始化
M(1,:)=Xkf(:,1);
P0=100e-2*eye(4);%协方差阵初始化


for i=2:N
    Xn=phi*Xkf(:,i-1);%预测
    M(i,:)=Xn;
    P1=phi*P0*phi'+Q;%预测误差协方差
    K=P1*H'*inv(H*P1*H'+R)%增益
    Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新
    P0=(eye(4)-K*H)*P1;             %滤波误差协方差更新
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(S(1,:),S(3,:),'g','LineWidth',1);%真实轨迹
% plot(X(1,:),X(3,:),'b','LineWidth',1);%观测轨迹
% plot(Z(1,:),Z(2,:),'r','LineWidth',1);%观测轨迹
% plot(Xkf(1,:),Xkf(3,:),'c','LineWidth',1);%卡尔曼滤波轨迹
% plot(M(1,:),M(3,:),'k','LineWidth',1);%一步预测轨迹
% legend('理论轨迹','实际运动轨迹','观测轨迹','滤波后轨迹','一步预测轨迹');
% xlabel('横坐标 X/m');
% ylabel('纵坐标 Y/m');
 
figure
hold on;box on;
plot(Err_Observation);
plot(Err_KalmanFilter);
legend('滤波前误差','滤波后误差');
xlabel('观测时间/s');
ylabel('误差值');

% 计算欧氏距离子函数
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