卡尔曼滤波辨识算法，MATLAB实现

clc; clear; close all; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% N = 10000; r = 4; t = 1:1:N; T = 1; x1 = zeros(1,N); x2 = zeros(1,N); x3 = zeros(1,N); X = zeros(1,N); Y = zeros(1,N); yy(1)=0; for n = 1:N % x1(n) =cos(0.001*n)^2; % x2(n) =sin(0.001*n)^2; % x3(n) =n^0.5/100; x1(n) =n*sin(n); x2(n) =2*n; x3(n) =n^0.5; xx(n)=5*x1(n)+4*x2(n)+3*x3(n); yy(n+1)=yy(n)+xx(n); end figure(1) plot(xx) hold on plot(yy) figure(2) plot(x1) hold on plot(x2) hold on plot(x3) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%----------生成数据 % 卡尔曼滤波部分，继承之前初始化变量 % A：转移矩阵 % H：量测矩阵 % Qk：系统噪声矩阵 % Rk：量测噪声矩阵 % P0：均方误差矩阵初始值 % Y：火箭的状态矩阵，由k_s，k_v,k_a组成 % Y0：状态矩阵的初始值 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% X =zeros(N,r); X(1,:) =[0 1 1 1]; H = [1 0 0 0]; Qk = [0.01 0 0 0;0 0.01 0 0 ;0 0 0.01 0;0 0 0 0.01 ]; Rk = 0.01; P0 = [0.01 0 0 0;0 0.01 0 0 ;0 0 0.01 0;0 0 0 0.01 ]; P1 = P0; P2 = zeros(r,r); for k = 1:N-1 A=[1 x1(k) x2(k) x3(k);0 1 0 0;0 0 1 0;0 0 0 1]; P2 = A*P1*A'+Qk; Kk = P2*H'*inv(H*P2*H'+Rk); P1 = (eye(r,r)-Kk*H)*P2; X(k+1,:) = X(k,:)+Kk'*(yy(k)-H*X(k+1,:)'); end figure(3) plot(X(:,2),'-'); hold on plot(X(:,3),':'); hold on plot(X(:,4),'k');