扩展卡尔曼滤波小程序

HYSYYRPS

% function  EXT Kalman Filtering
% vx[n]=vx[n-1]+ux[n]
% vy[n]=vy[n-1]+uy[n]
7 y' _3 z: e! |8 h+ \' z% rx[n]=rx[n-1]+vx[n-1]*t
% ry[n]=ry[n-1]+vy[n-1]*t
+ U0 t* a; q5 R, t  ?! h' l% s[n]=[rx[n];ry[n];vx[n];vy[n]]
: u' [! X2 D4 d- J1 R% s* f% A=[1 0 t 0;0 1 0 t;0 0 1 0;0 0 0 1]
% s[n-1]=[rx[n-1];ry[n-1];vx[n-1];vy[n-1]]
" P& C. F9 t; t  i+ M' V% u[n]=[0;0;ux[n];uy[n]]
%       s[n]=As*[n-1]+u[n]
( G! a& o, W2 s& b; P2 O%       x[n]=h(s[n])+w[n]

  \& y4 ?% y. y' b: o8 ^6 F. b% initialization
clear;
+ l5 M4 U0 |) g" h" Un=100;
A=[1 0 1 0;0 1 0 1;0 0 1 0;0 0 0 1];
) e, k7 `. r9 \rx=zeros(1,n); % 实际位置
ry=zeros(1,n); % 实际位置
  ^' Y% O1 Z# Yvx=zeros(1,n); % 实际速度
; [" x( N; w9 q9 h' |' @vy=zeros(1,n); % 实际速度
1 }! a0 R' D! `rxe=zeros(1,n); % 精确位置
rye=zeros(1,n); % 精确位置
8 |0 b- E" E: Ovxe=zeros(1,n); % 精确速度
, W3 p9 Z5 F( hvye=zeros(1,n); % 精确速度
rx(1,1)=10; % 位置初始
ry(1,1)=-5; % 位置初始
vx(1,1)=-0.2; % 速度初始
vy(1,1)=0.2; % 速度初始
rxe(1,1)=10;
rye(1,1)=-5;
  L4 P' ~5 v$ O* h8 H( Dvxe(1,1)=-0.2;
vye(1,1)=0.2;
s=[rx;ry;vx;vy];
se=[rxe;rye;vxe;vye];
r=zeros(1,n);
b=zeros(1,n);
  Z4 h( j7 ~, f) Z& h1 t, \rn=zeros(1,n);
re=zeros(1,n);
bn=zeros(1,n);
! e# n& z. E- O( {; yx=[rn;bn];
H=zeros(2,4,n);

ux=sqrt(0.00001)*randn(1,n);
: f3 M5 K) ~8 t' _* g! E! uuy=sqrt(0.00001)*randn(1,n);
wr=sqrt(0.001)*randn(1,n);
wb=sqrt(0.0001)*randn(1,n);

9 {" |9 s. {# SQ=[0 0 0 0;0 0 0 0;0 0 cov(ux) 0;0 0 0 cov(uy)]; % 过程噪声矩阵
C=[cov(wr) 0; 0 cov(wb)]; % 观测噪声矩阵
. g- T9 \- w$ Y1 h  I3 |- N1 i) r
* x, R" T. b8 }, F* t  mss=zeros(4,n);% 精确值
sn=zeros(4,n); % 估计修正值
sn(:,1)=sn(:,1)+[15;5;0;0]; % 估计修正初始值
ss=zeros(4,n); % 一步预测
hs=zeros(2,n); % 观测值预测
inn=zeros(2,n); % 新息
6 Q6 Z# Y0 g( e  i$ r4 s& ~, uM=zeros(4,4,n); % 估计均方误差
M(:,:,1)=M(:,:,1)+[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1]; % 估计均方误差初始值
MM=zeros(4,4,n); % 一步预测均方误差
K=zeros(4,2,n); % 卡尔曼增益

% kalman filtering
& n7 G2 d% q4 [7 Y
# h3 J. @9 n7 O4 ofor t=1:n-1
%            s[n]=As*[n-1]+u[n]
% k6 e. D( G% V; b7 H3 E; j%            x[n]=h(s[n])+w[n]

" I2 `! J' {7 a+ C' q    vx(1,t+1)=vx(1,t)+ux(1,t+1); % 速度过程方程
    vy(1,t+1)=vy(1,t)+uy(1,t+1); % 速度过程方程
! Y2 c3 W+ r, l/ j9 T, i    rx(1,t+1)=rx(1,t)+vx(1,t); % 位置过程方程
, b6 p- C, M- P    ry(1,t+1)=ry(1,t)+vy(1,t); % 位置过程方程
    s(:,t+1)=[rx(1,t+1);ry(1,t+1);vx(1,t+1);vy(1,t+1)]; % 过程矩阵，维数4*1
* g& m7 W% E; ?# w  d- ?
    vxe(1,t+1)=vxe(1,t); % 理论精确值
    vye(1,t+1)=vye(1,t); % 理论精确值
    rxe(1,t+1)=rxe(1,t)+vxe(1,t); % 理论精确值
- v0 b6 r1 q( G1 U5 j4 m& n    rye(1,t+1)=rye(1,t)+vye(1,t); % 理论精确值
    se(:,t+1)=[rxe(1,t+1);rye(1,t+1);vxe(1,t+1);vye(1,t+1)]; % 过程矩阵，维数4*1
* v( L1 B3 D( t, ~$ o1 j
    r(1,t+1)=(rx(1,t+1)^2+ry(1,t+1)^2)^(1/2); % 理论测量距离（含测量误差）
    b(1,t+1)=atan(ry(1,t+1)/rx(1,t+1)); % 理论测量方位（含测量误差）
    rn(1,t+1)=r(1,t+1)+wr(1,t+1); % 实际测量距离
1 N0 [" a, t1 r# e5 E- k- U9 A    re(1,t+1)=(rxe(1,t+1)^2+rye(1,t+1)^2)^(1/2); % 理论测量距离（含测量误差）
    bn(1,t+1)=b(1,t+1)+wb(1,t+1); % 实际测量方位
) \# r' j5 c' A% D4 n    x(:,t+1)=[rn(1,t+1);bn(1,t+1)]; % 观测矩阵,维数2*1
: I3 V& o" P: Q3 a4 [' E
. T" a% @9 D' }0 V    % 对观测方程求导,得到雅可比矩阵H，维数2*4
3 s* w- m) W" |7 q- A" m# n    H(:,:,t+1)=[rx(1,t+1)/((rx(1,t+1)^2+ry(1,t+1)^2)^(1/2)) ry(1,t+1)/((rx(1,t+1)^2+ry(1,t+1)^2)^(1/2)) 0 0;-ry(1,t+1)/(rx(1,t+1)^2+ry(1,t+1)^2) rx(1,t+1)/(rx(1,t+1)^2+ry(1,t+1)^2) 0 0];

) [: O3 o3 y6 q
7 l! z; d1 n+ ?7 G  b! o/ u, y7 l3 S    MM(:,:,t+1)=A*M(:,:,t)*A'+Q; % 一步预测均方误差
    K(:,:,t+1)=MM(:,:,t+1)*H(:,:,t+1)'/(C+H(:,:,t+1)*MM(:,:,t+1)*H(:,:,t+1)'); % 卡尔曼增益
' H) ~- x* ~2 m, L    M(:,:,t+1)=(eye(4)-K(:,:,t+1)*H(:,:,t+1))*MM(:,:,t+1);% 估计均方误差
, l. S3 k9 `5 L% R% M    ss(:,t+1)=A*sn(:,t); % 一步预测
    hs(:,t+1)=[((ss(1,t+1))^2+(ss(2,t+1))^2)^(1/2);atan(ss(2,t+1)/ss(1,t+1))]; % 观测值预测
    inn(:,t+1)=x(:,t+1)-hs(:,t+1); % 新息
    sn(:,t+1)=ss(:,t+1)+K(:,:,t+1)*inn(:,t+1); % 预测修正
3 p* d& e2 j- N* a' @5 dend

, [9 I9 e6 Z* q4 B" V9 T1 ]subplot(2,1,1);
2 k6 c$ ^8 G5 C3 K' ^2 oplot(sn(1,:),sn(2,:)); % kalman估计值
: t& i! w2 _9 L/ L0 Y  [" _hold on;
: H! b5 a& }6 j/ g' x, i6 v1 H* Cplot(se(1,:),se(2,:),'-r'); % 理论精确值

% v5 c' D% o4 H5 }; psubplot(2,1,2);
plot(rn); % 实际测量距离
% ^0 R+ i6 P* X# n3 ^3 _, Thold on;
plot(re,'-g'); % 理论精确值
; E9 H7 S1 F; R7 [

HYSYYRPS

这是一个位置和速度跟踪的小程序，感兴趣的可以看一下

HYSYYRPS

有没有同学是电力系统方向的，有的话，有没有做电力系统状态估计方向的，可以交流下