卡尔曼滤波小程序导航误差分析

卡尔曼滤波小程序，可用于去噪，GPS定位中定位精度的提高，

clear all
close all
a1 = -0.975;
a2 = 0.95;
N = 2;%2阶模型
N1 = 1000;%产生数据点数
N2 = 512;%取稳定后的输入数据个数
K = 100;%独立实验次数
c = 0.01;
Jmin = 0.005;%过程噪声协方差
var_v = 0.0731;%噪声方差
u = zeros（1N2）‘;
u1 = zeros（1N1）;
EJn = zeros（1N2）;
err = zeros（1N2）;
W1 = zeros（1N2）;
out_W1 = zeros（1N2）;
W2 = zeros（1N2）;
out_W2 = zeros（1N2）;
for i = 1:K
    v = sqrt（var_v）*randn（1N1）;
    %==========产生数据=============
    for m = 1:N1
        u1（m+2） = -a1*u1（m+1）-a2*u1（m）+v（m）;
    end
    u = u1（N1-N2+1:N1）;%取后面N2个稳定的数据
    %================Kalman滤波==================
    w = zeros（1N）.‘;%初始化最优权向量w（0）=w（1）=0即状态向量
    u3 = [zeros（1N） u].‘;
    I = eye（N）;%状态转移矩阵
    k = c.*I;%预测误差协方差
    k1 = zeros（N）;%一步预测误差协方差矩阵
    A = 0;%新息过程协方差
    G = zeros（1N）.‘;%Kalman增益
    a = 0;%定义新息过程
    for n = 1:N2
        k1 = k;
        u2 = u3（n+N-1:-1:n）;
        A = （u2‘）*k1*u2+Jmin;
        G = k1*u2*inv（A）;
        a = u（n）-（u2‘）*w;
        w = w+G*a;
        k = k1-G*（u2‘）*k1;
        err（n） = a;
        %abs（u（n）-（u2.‘）*conj（w））.^2;
        W1（n） = w（1）;
        W2（n） = w（2）;
    end
    EJn = EJn+（abs（err）.^2）;
    out_W1 = out_W1+W1;
    out_W2 = out_W2+W2;
end
EJn = EJn./K;
out_W1 = out_W1./K;
out_W2 = out_W2./K;
subplot（211）
plot（EJn）
grid on
title（‘Kalman滤波 a1=-0.975 a2=0.95 var_v=0.0731 c=0.01 Jmin=0.005‘）
gtext（‘均方误差学习曲线‘）
xlabel（‘迭代次数‘）;
ylabel（‘均方误差‘）;
%===========绘制权向量学习曲线==============
subplot（212）
plot（out_W1）;
gtext（‘w1‘）
grid on
hold on
plot（out_W2）;
gtext（‘w2‘）
xlabel（‘迭代次数‘）
ylabel（‘权值‘）
ylim（[-1.2 1.2]）
gtext（‘权值学习曲线‘）

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        1758  2009-05-08 19:14  KalmanAlgorithm.m