Kalman滤波器学习记录（三）

Kalman滤波的思考

Kalman的递推方差和逻辑已经得到，那么对于已知输入的控制量而言，可以将其加入状态转移阵，这将使的估计更准确些？或者是收敛快些？按照仿真的结果来看，效果不明显，但是可能有好处。似乎可以说只在乎状态量是如何变换的，因为可观性与输入是无关的。
除此之外的最大收获时当系统可观时，可以观测到全部的状态量，而避免了常用方法处理时带来的误差。以下时例子，运动的小车，加速度可以时恒定，也可以不定，只观测其位置，以往认为得到位置坐差分后可以得到速度，但其误差较大，使用kalman滤波可以使得速度误差下降明显，位置误差有些许下降，不过效果比低通滤波还是好，由于实际条件与数学假设不完全相同，因此也是正常的

以下附上matlab源码：
模型：匀加速或变加速的小车，以1m/s^2的加速度加速，每秒去观测：

close all;
clear;
T=0.01;%仿真时间0.01s
t=(1:10000)*0.01;
a=ones(1,10000)+wgn(1,10000,10*log10(0.1));%驱动及速度是1m/s^2，加上白色噪声，方差是0.1
%a=wgn(1,10000,10*log10(1));%变加速运动，方差是1
x(:,1)=[0;0];
for i=2:10000
    x(2,i)=sum(a(1,1:i-1))*T;%速度等于加速度的累计
end
for i=2:10000
    x(1,i)=sum(x(2,1:i-1))*T;%位移等于速度的累计
end

V=wgn(1,100,10*log10(10));%方差10
for i=1:100
    Z(1,i)=x(1,1+100*(i-1))+V(i);%观察值，加上观察噪声
    x_real(:,i)=x(:,1+100*(i-1));
end

%Kalman
%仅有位置观测值时，kalman滤波器预测位置有改进作用，但不特别明显。但对于速度，kalman的滤波效果远好于直接求平均速度
Ts=1;
ts=(1:100);
Q=[0.1,0;0,10];%系统噪声阵方差
R=10;%位置观察方差1m
F=[1,Ts;0,1];%状态传递阵
G=[Ts^2/2,0;0,Ts];%噪声驱动阵
H=[1,0];%观察阵
B=G;
u=[1;1];
xk_pre=[0;0];
xk(:,1)=[0;0];
Pk_pre=0*eye(2);
for i=2:100
    xk_=F*xk_pre+B*u;%状态外推%状态一步估计
    Pk_=F*Pk_pre*F'+G*Q*G';
    Kk=Pk_*H'/(H*Pk_*H'+R);
    %Pk_pre=inv(inv(Pk_)+H'/R*H);
    Pk_pre=(eye(2)-Kk*H)*Pk_;
    xk(:,i)=xk_+Kk*(Z(1,i)-H*xk_);
    xk_pre=xk(:,i);
end
%低通滤波
x_fil(1,1)=0;
for i=2:100
    x_fil(1,i)=0.01*x_fil(1,i-1)+0.99*Z(1,i);
    x_fil(2,i)=(x_fil(1,i)-x_fil(1,i-1));
end
for i=1:100
    err(1,i)=abs(xk(1,i)-x_real(1,i));%kalman位置滤波误差
    err(2,i)=abs(xk(2,i)-x_real(2,i));%kalman速度滤波误差
    err(3,i)=abs(Z(1,i)-x_real(1,i));%观察误差
    err(4,i)=abs(x_fil(1,i)-x_real(1,i));%低通滤波位置误差
    err(5,i)=abs(x_fil(2,i)-x_real(2,i));%低通滤波速度误差
end
% figure(1);
% hold on;box on;
% plot(ts,x_real(1,:),'-k');
% plot(ts,Z,'-b');
% plot(ts,xk(1,:),'-r+');
% legend('真实轨迹','观察轨迹','滤波轨迹');
% xlabel('时间');
% ylabel('位置');

figure(2);
hold on;box on;
plot(ts,err(1,:),'-k');
plot(ts,err(2,:),'-b');
plot(ts,err(3,:),'-r');
plot(ts,err(4,:),'-g');
plot(ts,err(5,:),'-y');
legend('kalman滤波位置误差','kalmam滤波速度误差','观察误差','低通滤波位置误差','低通滤波速度误差');
xlabel('时间');
ylabel('误差');