本文主要内容借鉴/heyijia0327/article/details/17487467博客当中的内容。
有一个匀加速运动的小车，它受到的合力为 ft ,由匀加速运动的位移和速度公式，能得到由 t-1 到 t 时刻的位移和速度变化公式：
该系统系统的状态向量包括位移和速度，分别用 xt 和 xt的导数 表示。控制输入变量为u，也就是加速度，于是有如下形式:
所以这个系统的状态的方程为：
这里对应的的矩阵A大小为 2*2 ,矩阵B大小为 2*1。
貌似有了这个模型就能完全估计系统状态了，速度能计算出，位移也能计算出。那还要卡尔曼干嘛，问题是很多实际系统复杂到根本就建不了模。并且，即使你建立了较为准确的模型，只要你在某一步有误差，由递推公式，很可能不断将你的误差放大A倍（A就是那个状态转换矩阵），以至于最后得到的估计结果完全不能用了。回到最开始的那个笑话，如果那个完全凭预测的特种兵在某一步偏离了正确的路径，当他站在错误的路径上（而他自己以为是正确的）做下一步预测时，肯定走的路径也会错了，到最后越走越偏。
既然如此，我们就引进反馈。从概率论贝叶斯模型的观点来看前面预测的结果就是先验，测量出的结果就是后验。
测量值当然是由系统状态变量映射出来的，方程形式如下：
注意Z是测量值，大小为m*1(不是n*1，也不是1*1，后面将说明），H也是状态变量到测量的转换矩阵。大小为m*n。随机变量v是测量噪声。
同时对于匀加速模型，假设下车是匀加速远离我们，我们站在原点用超声波仪器测量小车离我们的距离。
也就是测量值直接等于位移。可能又会问，为什么不直接用测量值呢？测量值噪声太大了，根本不能直接用它来进行计算。试想一个本来是朝着一个方向做匀加速运动的小车，你测出来的位移确是前后移动（噪声影响），只根据测量的结果，你就以为车子一会往前开一会往后开。
对于状态方程中的系统噪声w和测量噪声v，假设服从如下多元高斯分布，并且w,v是相互独立的。其中Q,R为噪声变量的协方差矩阵。
看到这里自然要提个问题，为什么噪声模型就得服从高斯分布呢？请继续往下看。
对于小车匀加速运动的的模型，假设系统的噪声向量只存在速度分量上，且速度噪声的方差是一个常量0.01，位移分量上的系统噪声为0。测量值只有位移，它的协方差矩阵大小是1*1，就是测量噪声的方差本身。
那么：
Q中，叠加在速度上系统噪声方差为0.01，位移上的为0，它们间协方差为0，即噪声间没有关联。
使用卡尔曼滤波器估计小车匀加速运动的
matlab程序如下：
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%日 期： .10.12%程序功能：使用卡尔曼滤波器估计小车匀加速运动%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear all % 初始化参数 delta_t=0.1; %采样时间 t=0:delta_t:5; N = length(t); % 序列的长度 sz = [2,N]; % 信号需开辟的内存空间大小 2行*N列 2:为状态向量的维数n g=10;%加速度值 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% x=1/2*g*t.^2;%实际真实位置 z = x + sqrt(10).*randn(1,N); % 测量时加入测量白噪声 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Q =[0 0;0 9e-1]; %假设建立的模型 噪声方差叠加在速度上 大小为n*n方阵 n=状态向量的维数 R = 10; % 位置测量方差估计，可以改变它来看不同效果 m*mm=z(i)的维数 A=[1 delta_t;0 1]; % n*n B=[1/2*delta_t^2;delta_t]; H=[1,0]; % m*n n=size(Q); %n为一个1*2的向量 Q为方阵 m=size(R); % 分配空间 xhat=zeros(sz); % x的后验估计 P=zeros(n); % 后验方差估计 n*n xhatminus=zeros(sz); % x的先验估计 Pminus=zeros(n);% n*n K=zeros(n(1),m(1)); % Kalman增益 n*m I=eye(n); % 估计的初始值都为默认的0，即P=[0 0;0 0],xhat=0 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%EKF过程for k = 9:N %假设车子已经运动9个delta_T了，我们才开始估计 % 时间更新过程 xhatminus(:,k) = A*xhat(:,k-1)+B*g; Pminus= A*P*A'+Q; % 测量更新过程 K = Pminus*H'*inv( H*Pminus*H'+R ); xhat(:,k) = xhatminus(:,k)+K*(z(k)-H*xhatminus(:,k)); P = (I-K*H)*Pminus; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% figure(1) plot(t,z); hold on plot(t,xhat(1,:),'r-') plot(t,x(1,:),'g-'); legend('含有噪声的测量', '后验估计', '真值'); xlabel('Iteration'); %% Estimate error(估计误差)x_error = x-xhat(1,:);%% Graph 2figure(2) plot(t,x_error(1,:)),grid on;title('位置误差')xlabel('时间 [sec]')ylabel('位置均方根误差 [m]')hold off;%% Graph 3figure(3)plot(t,xhat(2,:),'r'),grid on;title('实际速度 ')legend('实际速度')xlabel('时间 [sec]')ylabel('速度 [m/sec]')hold off;%==========================================================================
运行结果如下：
如果觉得《卡尔曼滤波估计小车匀加速运动》对你有帮助，请点赞、收藏，并留下你的观点哦！
