卡尔曼滤波（3）-----非线性处理

基本原理

以下图片来自何子述老师的《现代数字信号处理及其运用》

例子

源代码

%  函数功能：标量非线性系统扩展Kalman滤波问题
%  状态函数：X(k+1)=0.5X(k)+2.5X(k)/(1+X(k)^2)+8cos(1.2k) +w(k)
%  观测方程：Z（k）=X(k)^2/20+v(k)

clc;
clear all;
close all;
%---------参数设置
N=100;    % 仿真次数
t=0:N-1;  % 仿真时间

%-----------系统状态噪声、观测噪声
Q1=0.1;   % Q1的值改变，观察不同Q1值时滤波结果
Q2=10;    % 测量噪声        
v1=sqrt(Q1)*randn(1,N);% 产生过程噪声
v2=sqrt(Q2)*randn(1,N);% 产生观测噪声

%---------初始化
X=zeros(1,N);       % 状态方程初始化
X(1)=0.1;
Z=zeros(1,N);       % 输出方程初始化
Z(1)=X(1)^2/20+v2(1);
P0=eye(1);          % 协方差阵初始化
X_est=zeros(1,N);   % 卡尔曼估计状态
X_est(1)=X(1);      % 卡尔曼滤波状态给初值
X_err=zeros(1,N);   % 误差值初始化
X_err(1)=0;         % 误差值初始值为0 

for n=2:N %数字信号处理P263
    % 状态方程
    X(n)=0.5*X(n-1)+2.5*X(n-1)/(1+X(n-1)^2)+8*cos(1.2*n)+v1(n-1);
    % 观测方程
    Z(n)=X(n)^2/20+v2(n);
    
    %------开始卡尔曼滤波过程
    % 步骤1：状态一步预测
    X_pre=0.5*X_est(n-1)+2.5*X_est(n-1)/(1+X_est(n-1)^2)+8*cos(1.2*n);
    
    % 步骤2：一步预测误差自相关矩阵
    F_pre=0.5+2.5 *(1-X_pre^2)/(1+X_pre^2)^2;  % 状态转移矩阵更新 % 具体见7.5.1 
    P_pre=F_pre*P0*F_pre'+Q1;
    
    % 步骤3：卡尔曼增益 
    C=X_pre/10; % 观测矩阵  % 具体见7.5.2   
    K=P_pre*C'*inv(C*P_pre*C'+Q2);
    
    % 步骤4：状态估计
    Zn=X_pre^2/20;% 观测预测  ^2/20是函数h
    X_est(n)=X_pre+K*(Z(n)-Zn);  % 具体见教材P263
    
    % 步骤5：状态估计误差自相关矩阵
    P0=(1-K*C)*P_pre;
    
    %---------误差分析
    X_err(n)=abs( X_est(n)-X(n) );
end



%-----------画图
figure(1)
plot(t,X,'-ro');hold on;grid on;
plot(t,Z,'-.');hold on;grid on;
plot(t,X_est,'-b*');hold on;grid on;
xlabel('观察时间');
legend('真实的状态值','观测值','卡尔曼滤波后的值')
figure(2)
plot(t,X_err,'-b*');hold on;grid on;
xlabel('观察时间');ylabel('误差值');
title('误差图');

结果分析