扩展卡尔曼滤波的实现

亦凉 2022-08-14 04:50 161阅读 0赞

扩展卡尔曼滤波（Extended Kalman Filter ）与KF的最大的不同，是允许系统模型和测量模型非线性的存在，它的实现较为简单，参照Wikipedia，我把代码贴出来，方便学习交流。

采用一个简单的3阶非线性模型，仿真结果如下图：

![SouthEast][]

![SouthEast 1][]

源代码：

EKF\_Example:

**\[plain\]** [view plain][] [copy][view plain]

1.  clc; clear all; close all; 
2.  %% system and observation equation, with respect of their Jcobian function. 
3.  f\_model = @( x, u ) \[x(2);x(3);0.05\*x(1)\*(x(2)+x(3))\] + u; % nonlinear state equations 
4.  h\_model = @( x ) x(1); % measurement equation 
5.  f\_gradient = @ ( x, u ) ( \[ 0 1 0; 0 0 1; 0.05 \* ( x(2) + x(3)) 0.05 \* x(1) 0.05 \* x(1) \] ); 
6.  h\_gradient = @ ( x ) ( \[ 1 0 0 \] ); 
7.  %% Specify initial condition and parameters. 
8.  StateDim = 3; %number of state 
9.  ObvDim = 1; 
10. q = 0.1; %std of process 
11. r = 1.0; %std of measurement 
12. Q = q^2\*eye(StateDim); % covariance of process 
13. R = r^2; % covariance of measurement 
14. x0\_True = \[0;0;1\]; % initial state 
15. x0 = x0\_True + q \* randn(3,1); %initial state % initial state with noise 
16. L = 1;P0 = L \* eye(StateDim); % initial state covraiance 
17. MaxIter = 1000; % total dynamic steps 
18. %% EKF. 
19. \[ x\_Est P\_Est \] = EKF( f\_model, h\_model, f\_gradient, h\_gradient, x0, P0, Q, R, ObvDim, MaxIter );

EKF:

**\[plain\]** [view plain][] [copy][view plain]

1.  %% Extended Kalman Filter, and the equation is exactly same with Wikipedia(http://en.wikipedia.org/wiki/Extended\_Kalman\_filter) 
2.  %% this is the very basic algorithm, if you want get improved or variants EKF, please change corresponding modular in the following code. 
3.  function \[ x\_Est P\_Est \] = EKF( f\_model, h\_model, f\_gradient, h\_gradient, x0, P0, Q, R, ObvDim, MaxIter ) 
4.  % EKF Extended Kalman Filter for nonlinear dynamic systems 
5.  % this function returns state estimate, x and state covariance, P 
6.  % for nonlinear dynamic system: 
7.  % x\_k = f(x\_km1, ukm1) + w\_k 
8.  % z\_k = h(x\_k) + v\_k 
9.  % where w ~ N(0,Q) meaning w is gaussian noise with covariance Q 
10. % v ~ N(0,R) meaning v is gaussian noise with covariance R 
11. % Inputs: f\_model: function handle for f( x, u ) 
12. % h\_model: function handle for h( x ) 
13. % f\_gradient: f'( x, u ), evaluate f\_model's gradient at ( x, u ) 
14. % h\_gradient: h'( x ), evaluate h\_model's gradient at x. 
15. % x0: "a priori" state estimate 
16. % P0: "a priori" estimated state covariance 
17. % Q: process noise covariance of w\_k 
18. % R: measurement noise covariance of v\_k 
19. % ObvDim: z\_k's dimension 
20. % MaxIter: maximum iteration number. 
21. % Output: x\_Est: "a posteriori" state estimate matrix. 
22. % P\_Est: "a posteriori" state covariance matrix. 
23. % 
24. StateDim = length( x0 ); 
25. x\_True = \[ x0 \]; 
26. x\_Noise = \[ x0 \]; 
27. x\_Predict = \[ x0 \]; 
28. x\_Est = \[ x0 \]; 
29. u = zeros( StateDim, MaxIter ); 
30. z\_True = \[\]; 
31. z\_Noise = \[\]; 
32. P\_Predict = \[\]; 
33. P\_Est( :, :, 1 ) = P0; 
34. for k = 2 : 1 : MaxIter 
35.  %% this part is only to generate true data and noise data, in practice, you will only get noise data as input. 
36.  % generate true data. 
37.  x\_True( :, k ) = f\_model( x\_True( :, k - 1 ), u( :, k - 1 ) ); 
38.  Z\_True( :, k ) = h\_model( x\_True( :, k ) ); 
39.  % Add noise, and these are true measurements value. 
40.  Wk = mvnrnd( zeros( StateDim, 1 ), Q ); 
41.  x\_Noise( :, k ) = x\_True( :, k ) + Wk'; 
42.  Vk = mvnrnd( zeros( ObvDim, 1 ), R ); 
43.  z\_Noise( :, k ) = Z\_True( :, k ) + Vk'; 
44.  %% the following is EKF. 
45.  % Predict, take the last step's state to predict current state. 
46.  x\_Predict( :, k ) = f\_model( x\_Est( :, k - 1 ), u( :, k - 1 ) ); % predicted state estimate. 
47.  F = f\_gradient( x\_Est( :, k - 1 ), u( :, k - 1 ) ); 
48.  P\_Predict( :, :, k ) = F \* P\_Est( :, :, k - 1 ) \* F' + Q; % predict covariance estimate. 
49.  % Update, besides predicted state calculated above, we incorporate 
50.  %measurements to get a more general state estimate. 
51.  y(:, k ) = z\_Noise( :, k ) - h\_model( x\_Predict( :, k ) ); % innovation or measurement residual. 
52.  H = h\_gradient( x\_Predict( :, k - 1 ) ); 
53.  S(:, :, k ) = H \* P\_Predict( :, :, k ) \* H' + R; % innovation covariance. 
54.  K( :, :, k ) = P\_Predict( :, :, k ) \* H' \* inv( S( :, :, k ) ); % Near-optimal Kalman gain. 
55.  x\_Est(:, k ) = x\_Predict( :, k ) + K( :, :, k ) \* y( :, k ); % Update state estimate, this is what you need. 
56.  P\_Est( :, :, k ) = ( eye(StateDim) - K( :, :, k ) \* H ) \* P\_Predict( :, :, k ); % update covariance estimate. 
57. end 
58. % RMS error metric. 
59. RMS = mean( ( x\_Est - x\_True ).^2, 2 ) 
60. %% Visualize. 
61. figure(2); 
62. plot( x\_True( 1, : ), 'b\*' ); 
63. hold on;plot( x\_Est( 1, : ), 'g\*' ); 
64. hold on; plot( z\_Noise( 1, : ), 'r\*' ); 
65. legend('True State', 'Optimal State', 'Measurement' ); 
66. title( ' Extended Kalman Filter '); 
67. xlabel('Time(s)');ylabel( 'Position(m)' ); 
68. grid on; 
69. end 
70.

[SouthEast]: /images/20220810/60916bc8c5f74e83a4bffcac07f850ec.png
[SouthEast 1]: /images/20220810/a9d5cc57ed3d419cbdba21e42236523d.png
[view plain]: http://blog.csdn.net/lump_night5/article/details/39403685#