matlab 实现的kalman滤波

clear clc close all %kalmanf_example_2 %x列向量 % x(k+1)=[1 T; 0 1]x(k)+[T^2/2;T]v(k); % z(k)=[1 0]x(k)+w(k); % v(k)~N(0, 1) Q; % w(k)~N(0, 1) R; % x(0)~N([0; -10], I) T=0.025; F=[1 T; 0 1]; TN=[T^2/2;T]; G=[0 0; 0 0]; u=[0;0]; H=[1 0]; Q=1; R=1; %初始状态以及协方差 x=[0; -10]; P=[1 0; 0 1]; tru=[0; -10];%记录真实状态 z=[0]; iteration=500; chi=[0]; for t=1:iteration %% tru(:,end+1)=F*tru(:,end)+TN*randn;%真实状态,受过程噪声影响. %对实际系统来说,系统的真实状态是不可知的,在实验中采用一些方法来获取相对 %更精确的状态来近似作为真实状态,例如使用相对精度更高的传感器来近似系统状态. %而在仿真中,要模拟系统的状态,系统的模型中的噪声包括两方面,一个是系统本身所受 %到的噪声干扰(计算真实状态时需加入),还需要考虑建模的不确定性(计算真实状态时不加入), %仿真中为了方便,就将噪声统一在一起,但由于都是由随机数生成,可以用来近似系统过程噪声. %% z(end+1)=H*tru(:,end)+randn; %构造量测值 %% %预测 xbar=F*x(:,end)+G*u;%状态预测值 zbar=H*xbar; %量测预测值 Pbar=F*P(:,end-1: