卡尔曼滤波

4.MATLAB源代码： （1）UKF源代码：

function [x,P]=ukf(fstate,x,P,hmeas,z,Q,R)

% UKF Unscented Kalman Filter for nonlinear dynamic systems % [x, P] = ukf(f,x,P,h,z,Q,R) returns state estimate, x and state covariance, P

% for nonlinear dynamic system (for simplicity, noises are assumed as additive):

% x_k+1 = f(x_k) + w_k % z_k = h(x_k) + v_k

% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q % v ~ N(0,R) meaning v is gaussian noise with covariance R % Inputs:

% f: function handle for f(x) % x: \

% P: \% h: fanction handle for h(x) % z: current measurement % Q: process noise covariance % R: measurement noise covariance

% Output: x: \% P: \L=numel(x); %状态向量的个数 m=numel(z); %测量状态向量的个数 alpha=1e-3; Tfault, tunable ki=0; Tfault, tunable beta=2; Tfault, tunable

lambda=alpha^2*(L+ki)-L; %scaling factor c=L+lambda; %scaling factor

Wm=[lambda/c 0.5/c+zeros(1,2*L)]; %weights for means Wc=Wm;

Wc(1)=Wc(1)+(1-alpha^2+beta); %weights for covariance c=sqrt(c);

X=sigmas(x,P,c); %sigma points around x

[x1,X1,P1,X2]=ut(fstate,X,Wm,Wc,L,Q); %unscented transformation of process

% X1=sigmas(x1,P1,c); %sigma points around x1 % X2=X1-x1(:,ones(1,size(X1,2))); Tviation of X1

[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R); %unscented transformation of measurments

P12=X2*diag(Wc)*Z2'; %transformed cross-covariance

K=P12*inv(P2);

x=x1+K*(z-z1); %state update P=P1-K*P12'; %covariance update function [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R) %Unscented Transformation %Input:

% f: nonlinear map % X: sigma points % Wm: weights for mean % Wc: weights for covraiance % n: numer of outputs of f % R: additive covariance %Output:

% y: transformed mean

% Y: transformed smapling points % P: transformed covariance % Y1: transformed deviations L=size(X,2); y=zeros(n,1); Y=zeros(n,L); for k=1:L

Y(:,k)=f(X(:,k)); y=y+Wm(k)*Y(:,k); end

Y1=Y-y(:,ones(1,L)); P=Y1*diag(Wc)*Y1'+R; function X=sigmas(x,P,c)

%Sigma points around reference point %Inputs:

% x: reference point % P: covariance % c: coefficient %Output:

% X: Sigma points A = c*chol(P)';

Y = x(:,ones(1,numel(x))); X = [x Y+A Y-A];

（2）输入文件源代码：

%n=3; %number of state clc; clear; n=3; t=0.1;

q=0.2; %std of process r=0.3; %std of measurement

Q=q^2*eye(n); % covariance of process R=r^2; % covariance of measurement

%f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equations f=@(x)[x(1)+t*x(2);x(2)+t*x(3);x(3)]; % nonlinear state equations h=@(x)[0;x(2);0]; % measurement equation %s=[0;0;1]; % initial state s=[0;0;1];

x=s+q*randn(3,1); %initial state % initial state with noise P = eye(n); % initial state covraiance N=70; % total dynamic steps

xV = zeros(n,N); %estmate % allocate memory sV = zeros(n,N); ?tual zV = zeros(3,N); for k=1:N

z = h(s) + r*randn; % measurments sV(:,k)= s; % save actual state zV(:,k) = z; % save measurment [x, P] = ukf(f,x,P,h,z,Q,R); % ukf xV(:,k) = x; % save estimate

s = f(s) + q*randn(3,1); % update process end

for k=1:3 % plot results subplot(3,1,k)

plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--',1:N,zV(k,:),'*') end

%n=3; %number of state clc; clear; n=6; t=0.2;

q=0.1; %std of process r=0.7; %std of measurement

Q=q^2*eye(n); % covariance of process R=r^2; % covariance of measurement

%f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equations f=@(x)[x(1)+t*x(3);x(2)+t*x(4);x(3)+t*x(5);x(4)+t*x(6);x(5);x(6)]; % nonlinear state equations

h=@(x)[sqrt(x(1)+1);0.8*x(2)+0.3*x(1);x(3);x(4);x(5);x(6)]; % measurement equation %s=[0;0;1]; % initial state s=[0.3;0.2;1;2;2;-1];

x=s+q*randn(n,1); %initial state % initial state with noise

P = eye(n); % initial state covraiance N=20; % total dynamic steps

xV = zeros(n,N); %estmate % allocate memory sV = zeros(n,N); ?tual zV = zeros(n,N); for k=1:N

z = h(s) + r*randn; % measurments sV(:,k)= s; % save actual state zV(:,k) = z; % save measurment [x, P] = ukf(f,x,P,h,z,Q,R); % ukf xV(:,k) = x; % save estimate

s = f(s) + q*randn(n,1); % update process end

for k=1:4 % plot results subplot(4,1,k)

plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--',1:N,zV(k,:),'*') end

本文来源：https://www.bwwdw.com/article/pvcv.html