KF+EKF matlab程序实现

主要关于卡尔曼滤波与扩展卡尔曼滤波，包含两个KF程序、一个EKF程序

clear;  
clc;

%%  真实轨迹模拟  
kx = .01;   ky = .05;       % 阻尼系数  
g = 9.8;                    % 重力  
t = 15;                     % 仿真时间  
Ts = 0.1;                   % 采样周期   
len = fix（t/Ts）;            % 仿真步数  
dax = 3; day = 3;           % 系统噪声  X、Y两个状态下的噪声
X = zeros（len4）;   
X（1:） = [0 50 500 0];   % 状态模拟的初值  

%% 构造状态方程，作为实际数据，用作对比
for k=2:len  
    x = X（k-11）; vx = X（k-12）; y = X（k-13）; vy = X（k-14）;   %定义系统状态
    
    x = x + vx*Ts;  
    vx = vx + （-kx*vx^2+dax*randn（11））*Ts;  
    y = y + vy*Ts;  
    vy = vy + （ky*vy^2-g+day*randn（1））*Ts;  
    
    X（k:） = [x vx y vy];  
end   

%%  构造观测方程，作为观测数据
dr = 8;  dafa = 0.1;        % 量测噪声  
for k=1:len  
    r = sqrt（X（k1）^2+X（k3）^2） + dr*randn（11）;  
    a = atan（X（k1）/X（k3））*57.3 + dafa*randn（11）;  
    Z（k:） = [r a];  
end  

%% ekf 滤波  
Qk = diag（[0; dax/10; 0; day/10]）^2;  %系统噪声协方差矩阵  
Rk = diag（[dr; dafa]）^2;              %量测噪声协方差矩阵  
Pk = 10*eye（4）;                       %误差协方差矩阵
Pkk_1 = 10*eye（4）;                    
x_hat = [0404000]‘;                %状态初始化
X_est = zeros（len4）;  

x_forecast = zeros（41）;              %预测矩阵
z = zeros（41）;                       %观测矩阵

for k=1:len  
    % 公式1  状态预测      
    x1 = x_hat（1） + x_hat（2）*Ts;  
    vx1 = x_hat（2） + （-kx*x_hat（2）^2）*Ts;  
    y1 = x_hat（3） + x_hat（4）*Ts;  
    vy1 = x_hat（4） + （ky*x_hat（4）^2-g）*Ts;  
    x_forecast = [x1; vx1; y1; vy1];        %预测值  
    
    % 观测量预测  
    r = sqrt（x1*x1+y1*y1）;  
    alpha = atan（x1/y1）*57.3;  
    y_yuce = [ralpha]‘;  
    
    %  状态矩阵  
    vx = x_forecast（2）;  vy = x_forecast（4）;  
    F = zeros（44）;                        %定义线性化状态矩阵
    F（11） = 1;  F（12） = Ts;  
    F（22） = 1-2*kx*vx*Ts;  
    F（33） = 1;  F（34） = Ts;  
    F（44） = 1+2*ky*vy*Ts;  
    Pkk_1 = F*Pk*F‘+Qk;                    %公式2，误差协方差预测
    
    % 观测矩阵  
    x = x_forecast（1）; y = x_forecast（3）;  
    H = zeros（24）;                        %定义线性化观测矩阵
    r = sqrt（x^2+y^2）;  xy2 = 1+（x/y）^2;  
    H（11） = x/r;  H（13） = y/r;  
    H（21） = （1/y）/xy2;  H（23） = （-x/y^2）/xy2;  
      
    Kk = Pkk_1*H‘*（H*Pkk_1*H‘+Rk）^-1;       %公式3，计算增益K  
    x_hat = x_forecast+Kk*（Z（k:）‘-y_yuce）; %公式4，状态更新  
    Pk = （eye（4）-Kk*H）*Pkk_1;               %公式5，误差协方差更新 
    X_est（k:） = x_hat;  
end 

%% 绘图   
figure hold on grid on;  
plot（X（:1）X（:3）‘-b‘）;  
plot（Z（:1）.*sin（Z（:2）*pi/180） Z（:1）.*cos（Z（:2）*pi/180））;  
plot（X_est（:1）X_est（:3） ‘r‘）;  
xlabel（‘X‘）;   
ylabel（‘Y‘）;   
title（‘EKF simulation‘）;  
legend（‘real‘ ‘measurement‘ ‘ekf estimated‘）;  
axis（[-5230290530]）;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        2010  2018-06-22 20:00  example2_KF.m
     文件        1522  2018-06-22 21:06  example3_KF.m
     文件        2870  2018-06-22 11:14  example1_EKF.m