终于成功仿了一次Kalman滤波器
首先是测试了从网上down的一段代码
%
KALMANF
-
updates a system state vector estimate based upon an
%
observation,
using
a discrete Kalman filter.
%
%
Version
1.0
, June
30
,
2004
%
%
This tutorial function was written by Michael C. Kleder
%
(Comments are appreciated at:
public
@kleder.com)
%
%
INTRODUCTION
%
%
Many people have heard of Kalman filtering, but regard the topic
%
as
mysterious. While it
'
s true that deriving the Kalman filter and
%
proving mathematically that it
is
"
optimal
"
under a variety of
%
circumstances can be rather intense, applying the filter to
%
a basic linear system
is
actually very easy. This Matlab file
is
%
intended to demonstrate that.
%
%
An excellent paper on Kalman filtering at the introductory level,
%
without detailing the mathematical underpinnings,
is
:
%
"
An Introduction to the Kalman Filter
"
%
Greg Welch and Gary Bishop, University of North Carolina
%
http:
//
www.cs.unc.edu/~welch/kalman/kalmanIntro.html
%
%
PURPOSE:
%
%
The purpose of each iteration of a Kalman filter
is
to update
%
the estimate of the state vector of a system (and the covariance
%
of that vector) based upon the information
in
a
new
observation.
%
The version of the Kalman filter
in
this
function assumes that
%
observations occur at
fixed
discrete time intervals. Also,
this
%
function assumes a linear system, meaning that the time evolution
%
of the state vector can be calculated by means of a state transition
%
matrix.
%
%
USAGE:
%
%
s
=
kalmanf(s)
%
%
"
s
"
is
a
"
system
"
struct
containing various fields used
as
input
%
and output. The state estimate
"
x
"
and its covariance
"
P
"
are
%
updated by the function. The other fields describe the mechanics
%
of the system and are left unchanged. A calling routine may change
%
these other fields
as
needed
if
state dynamics are time
-
dependent;
%
otherwise, they should be left alone after initial values are
set
.
%
The exceptions are the observation vectro
"
z
"
and the input control
%
(or forcing function)
"
u.
"
If there
is
an input function, then
%
"
u
"
should be
set
to some nonzero value by the calling routine.
%
%
SYSTEM DYNAMICS:
%
%
The system evolves according to the following difference equations,
%
where quantities are further defined below:
%
%
x
=
Ax
+
Bu
+
w meaning the state vector x evolves during one time
%
step by premultiplying by the
"
state transition
%
matrix
"
A. There is optionally (if nonzero) an input
%
vector u which affects the state linearly, and
this
%
linear effect on the state
is
represented by
%
premultiplying by the
"
input matrix
"
B. There
is
also
%
gaussian process noise w.
%
z
=
Hx
+
v meaning the observation vector z
is
a linear function
%
of the state vector, and
this
linear relationship
is
%
represented by premultiplication by
"
observation
%
matrix
"
H. There is also gaussian measurement
%
noise v.
%
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
%
%
VECTOR VARIABLES:
%
%
s.x
=
state vector estimate. In the input
struct
,
this
is
the
%
"
a priori
"
state estimate (prior to the addition of the
%
information from the
new
observation). In the output
struct
,
%
this
is
the
"
a posteriori
"
state estimate (after the
new
%
measurement information
is
included).
%
s.z
=
observation vector
%
s.u
=
input control vector, optional (defaults to zero).
%
%
MATRIX VARIABLES:
%
%
s.A
=
state transition matrix (defaults to identity).
%
s.P
=
covariance of the state vector estimate. In the input
struct
,
%
this
is
"
a priori,
"
and
in
the output it
is
"
a posteriori.
"
%
(required unless autoinitializing
as
described below).
%
s.B
=
input matrix, optional (defaults to zero).
%
s.Q
=
process noise covariance (defaults to zero).
%
s.R
=
measurement noise covariance (required).
%
s.H
=
observation matrix (defaults to identity).
%
%
NORMAL OPERATION:
%
%
(
1
) define all state definition fields: A,B,H,Q,R
%
(
2
) define intial state estimate: x,P
%
(
3
) obtain observation and control vectors: z,u
%
(
4
) call the filter to obtain updated state estimate: x,P
%
(
5
)
return
to step (
3
) and repeat
%
%
INITIALIZATION:
%
%
If an initial state estimate
is
unavailable, it can be obtained
%
from the first observation
as
follows, provided that there are the
%
same number of observable variables
as
state variables. This
"
auto-
%
intitialization
"
is done automatically if s.x is absent or NaN.
%
%
x
=
inv(H)
*
z
%
P
=
inv(H)
*
R
*
inv(H
'
)
%
%
This
is
mathematically equivalent to setting the initial state estimate
%
covariance to infinity.
%
%
SCALAR EXAMPLE (Automobile Voltimeter):
%
%
%
Define the system
as
a constant of
12
volts:
function T
clear s
s.x
=
12
;
s.A
=
1
;
%
%
Define a process noise (stdev) of
2
volts
as
the car operates:
s.Q
=
2
^
2
;
%
variance, hence stdev
^
2
%
Define the voltimeter to measure the voltage itself:
s.H
=
1
;
%
%
Define a measurement error (stdev) of
2
volts:
s.R
=
2
^
2
;
%
variance, hence stdev
^
2
%
Do not define any system input (control) functions:
s.B
=
0
;
s.u
=
0
;
%
%
Do not specify an initial state:
s.x
=
nan;
s.P
=
nan;
%
%
Generate random voltages and watch the filter operate.
tru
=
[];
%
truth voltage
for
t
=
1
:
20
tru(end
+
1
)
=
randn
*
2
+
12
;
s(end).z
=
tru(end)
+
randn
*
2
;
%
create a measurement
s(end
+
1
)
=
kalmanf(s(end));
%
perform a Kalman filter iteration
%
end
%
figure
%
hold on
%
grid on
%
%
plot measurement data:
hz
=
plot([s(
1
:end
-
1
).z],
'
r
'
);hold on
%
%
plot a
-
posteriori state estimates:
hk
=
plot([s(
2
:end).x],
'
b-
'
);hold on
ht
=
plot(tru,
'
g-
'
);hold on
legend(
'
observations
'
,
'
Kalman output
'
,
'
true voltage
'
,
0
)
title(
'
Automobile Voltimeter Example
'
)
%
hold off
end
function s
=
kalmanf(s)
%
set
defaults
for
absent fields:
if
~
isfield(s,
'
x
'
); s.x
=
nan
*
z; end
if
~
isfield(s,
'
P
'
); s.P
=
nan; end
if
~
isfield(s,
'
z
'
); error(
'
Observation vector missing
'
); end
if
~
isfield(s,
'
u
'
); s.u
=
0
; end
if
~
isfield(s,
'
A
'
); s.A
=
eye(length(x)); end
if
~
isfield(s,
'
B
'
); s.B
=
0
; end
if
~
isfield(s,
'
Q
'
); s.Q
=
zeros(length(x)); end
if
~
isfield(s,
'
R
'
); error(
'
Observation covariance missing
'
); end
if
~
isfield(s,
'
H
'
); s.H
=
eye(length(x)); end
if
isnan(s.x)
%
initialize state estimate from first observation
if
diff(size(s.H))
error(
'
Observation matrix must be square and invertible for state autointialization.
'
);
end
s.x
=
inv(s.H)
*
s.z;
s.P
=
inv(s.H)
*
s.R
*
inv(s.H
'
);
else
%
This
is
the code which implements the discrete Kalman filter:
%
Prediction
for
state vector and covariance:
s.x
=
s.A
*
s.x
+
s.B
*
s.u;
s.P
=
s.A
*
s.P
*
s.A
'
+ s.Q;
%
Compute Kalman gain factor:
K
=
s.P
*
s.H
'
*inv(s.H*s.P*s.H
'
+
s.R);
%
Correction based on observation:
s.x
=
s.x
+
K
*
(s.z
-
s.H
*
s.x);
s.P
=
s.P
-
K
*
s.H
*
s.P;
%
Note that the desired result, which
is
an improved estimate
%
of the sytem state vector x and its covariance P, was obtained
%
in
only five lines of code, once the system was defined. (That
'
s
%
how simple the discrete Kalman filter
is
to use.) Later,
%
we
'
ll discuss how to deal with nonlinear systems.
end
后来不过瘾，自己写了一个，没想到稍微改了一改竟然成功了，效果还不错
% 状态
% xk=A•xk-1+B•uk+wk
% zk=H•xk+vk,
% p(w) ~ N(0,Q)
% p(v) ~ N(0,R),
% 预测
% x'k=A•xk+B•uk
% P'k=A•P(k-1)*AT + Q
% 修正
% Kk=P'k•HT•(H•P'k•HT+R)-1
% xk=x'k+Kk•(zk-H•x'k)
% Pk=(I-Kk•H)•P'k
%要注意的是：必须把系统状态和kalman滤波器内部预测的状态分开
function Test
A=[1 0.1;0 1];
B=0;
Xp=rand(2,1)*0.1;
X=[0 0]';
H=[1 0];
Q=eye(2)*1e-5;
R=eye(1)*0.1;
P=eye(2);% P'(k)
angle=[];
angle_m=[];
angle_real=[];
for i=1:500
angle_real=[angle_real X(1)]; %实际角度
[Xp,P]=Predict(A,Xp,P,Q);
X=A*X+rand(2,1)*1e-5;
z_m=H*X+rand(1,1)*0.1-0.05;
angle_m=[angle_m z_m(1)]; %测量的角度
[Xp,P]=Correct(P,H,R,X,z_m);
angle=[angle Xp(1)]; %预测的角度
end
t=1:500;
plot(t,angle,'r',t,angle_m,'g',t,angle_real,'b')
legend('预测值','测量值','实际值')
figure
plot(t,angle-angle_real,'r',t,angle_m-angle_real,'g')
legend('滤波后的误差','测量的误差')
title('误差分析')
xlabel('time');
ylabel('error');
function [Xk,Pk]=Predict(A,Xk,Pk_1,Q)
Xk=A*Xk;
Pk=A*Pk_1*A'+Q;
function [Xk,Pk]=Correct(Pk,H,R,Xk,zk)
Kk=Pk * H' * inv(H * Pk * H' + R);
Xk=Xk+ Kk*(zk-H*Xk);
Pk=(eye(size(Pk,1)) - Kk*H)*Pk;
%
KALMANF
-
updates a system state vector estimate based upon an
%
observation,
using
a discrete Kalman filter.
%
%
Version
1.0
, June
30
,
2004
%
%
This tutorial function was written by Michael C. Kleder
%
(Comments are appreciated at:
public
@kleder.com)
%
%
INTRODUCTION
%
%
Many people have heard of Kalman filtering, but regard the topic
%
as
mysterious. While it
'
s true that deriving the Kalman filter and
%
proving mathematically that it
is
"
optimal
"
under a variety of
%
circumstances can be rather intense, applying the filter to
%
a basic linear system
is
actually very easy. This Matlab file
is
%
intended to demonstrate that.
%
%
An excellent paper on Kalman filtering at the introductory level,
%
without detailing the mathematical underpinnings,
is
:
%
"
An Introduction to the Kalman Filter
"
%
Greg Welch and Gary Bishop, University of North Carolina
%
http:
//
www.cs.unc.edu/~welch/kalman/kalmanIntro.html
%
%
PURPOSE:
%
%
The purpose of each iteration of a Kalman filter
is
to update
%
the estimate of the state vector of a system (and the covariance
%
of that vector) based upon the information
in
a
new
observation.
%
The version of the Kalman filter
in
this
function assumes that
%
observations occur at
fixed
discrete time intervals. Also,
this
%
function assumes a linear system, meaning that the time evolution
%
of the state vector can be calculated by means of a state transition
%
matrix.
%
%
USAGE:
%
%
s
=
kalmanf(s)
%
%
"
s
"
is
a
"
system
"
struct
containing various fields used
as
input
%
and output. The state estimate
"
x
"
and its covariance
"
P
"
are
%
updated by the function. The other fields describe the mechanics
%
of the system and are left unchanged. A calling routine may change
%
these other fields
as
needed
if
state dynamics are time
-
dependent;
%
otherwise, they should be left alone after initial values are
set
.
%
The exceptions are the observation vectro
"
z
"
and the input control
%
(or forcing function)
"
u.
"
If there
is
an input function, then
%
"
u
"
should be
set
to some nonzero value by the calling routine.
%
%
SYSTEM DYNAMICS:
%
%
The system evolves according to the following difference equations,
%
where quantities are further defined below:
%
%
x
=
Ax
+
Bu
+
w meaning the state vector x evolves during one time
%
step by premultiplying by the
"
state transition
%
matrix
"
A. There is optionally (if nonzero) an input
%
vector u which affects the state linearly, and
this
%
linear effect on the state
is
represented by
%
premultiplying by the
"
input matrix
"
B. There
is
also
%
gaussian process noise w.
%
z
=
Hx
+
v meaning the observation vector z
is
a linear function
%
of the state vector, and
this
linear relationship
is
%
represented by premultiplication by
"
observation
%
matrix
"
H. There is also gaussian measurement
%
noise v.
%
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
%
%
VECTOR VARIABLES:
%
%
s.x
=
state vector estimate. In the input
struct
,
this
is
the
%
"
a priori
"
state estimate (prior to the addition of the
%
information from the
new
observation). In the output
struct
,
%
this
is
the
"
a posteriori
"
state estimate (after the
new
%
measurement information
is
included).
%
s.z
=
observation vector
%
s.u
=
input control vector, optional (defaults to zero).
%
%
MATRIX VARIABLES:
%
%
s.A
=
state transition matrix (defaults to identity).
%
s.P
=
covariance of the state vector estimate. In the input
struct
,
%
this
is
"
a priori,
"
and
in
the output it
is
"
a posteriori.
"
%
(required unless autoinitializing
as
described below).
%
s.B
=
input matrix, optional (defaults to zero).
%
s.Q
=
process noise covariance (defaults to zero).
%
s.R
=
measurement noise covariance (required).
%
s.H
=
observation matrix (defaults to identity).
%
%
NORMAL OPERATION:
%
%
(
1
) define all state definition fields: A,B,H,Q,R
%
(
2
) define intial state estimate: x,P
%
(
3
) obtain observation and control vectors: z,u
%
(
4
) call the filter to obtain updated state estimate: x,P
%
(
5
)
return
to step (
3
) and repeat
%
%
INITIALIZATION:
%
%
If an initial state estimate
is
unavailable, it can be obtained
%
from the first observation
as
follows, provided that there are the
%
same number of observable variables
as
state variables. This
"
auto-
%
intitialization
"
is done automatically if s.x is absent or NaN.
%
%
x
=
inv(H)
*
z
%
P
=
inv(H)
*
R
*
inv(H
'
)
%
%
This
is
mathematically equivalent to setting the initial state estimate
%
covariance to infinity.
%
%
SCALAR EXAMPLE (Automobile Voltimeter):
%
%
%
Define the system
as
a constant of
12
volts:function Tclear ss.x
=
12
;s.A
=
1
;
%
%
Define a process noise (stdev) of
2
volts
as
the car operates:s.Q
=
2
^
2
;
%
variance, hence stdev
^
2
%
Define the voltimeter to measure the voltage itself:s.H
=
1
;
%
%
Define a measurement error (stdev) of
2
volts:s.R
=
2
^
2
;
%
variance, hence stdev
^
2
%
Do not define any system input (control) functions:s.B
=
0
;s.u
=
0
;
%
%
Do not specify an initial state:s.x
=
nan;s.P
=
nan;
%
%
Generate random voltages and watch the filter operate.tru
=
[];
%
truth voltage
for
t
=
1
:
20
tru(end
+
1
)
=
randn
*
2
+
12
; s(end).z
=
tru(end)
+
randn
*
2
;
%
create a measurement s(end
+
1
)
=
kalmanf(s(end));
%
perform a Kalman filter iteration
%
end
%
figure
%
hold on
%
grid on
%
%
plot measurement data: hz
=
plot([s(
1
:end
-
1
).z],
'
r
'
);hold on
%
%
plot a
-
posteriori state estimates: hk
=
plot([s(
2
:end).x],
'
b-
'
);hold on ht
=
plot(tru,
'
g-
'
);hold on legend(
'
observations
'
,
'
Kalman output
'
,
'
true voltage
'
,
0
) title(
'
Automobile Voltimeter Example
'
)
%
hold offend function s
=
kalmanf(s)
%
set
defaults
for
absent fields:
if
~
isfield(s,
'
x
'
); s.x
=
nan
*
z; end
if
~
isfield(s,
'
P
'
); s.P
=
nan; end
if
~
isfield(s,
'
z
'
); error(
'
Observation vector missing
'
); end
if
~
isfield(s,
'
u
'
); s.u
=
0
; end
if
~
isfield(s,
'
A
'
); s.A
=
eye(length(x)); end
if
~
isfield(s,
'
B
'
); s.B
=
0
; end
if
~
isfield(s,
'
Q
'
); s.Q
=
zeros(length(x)); end
if
~
isfield(s,
'
R
'
); error(
'
Observation covariance missing
'
); end
if
~
isfield(s,
'
H
'
); s.H
=
eye(length(x)); end
if
isnan(s.x)
%
initialize state estimate from first observation
if
diff(size(s.H)) error(
'
Observation matrix must be square and invertible for state autointialization.
'
); end s.x
=
inv(s.H)
*
s.z; s.P
=
inv(s.H)
*
s.R
*
inv(s.H
'
);
else
%
This
is
the code which implements the discrete Kalman filter:
%
Prediction
for
state vector and covariance: s.x
=
s.A
*
s.x
+
s.B
*
s.u; s.P
=
s.A
*
s.P
*
s.A
'
+ s.Q;
%
Compute Kalman gain factor: K
=
s.P
*
s.H
'
*inv(s.H*s.P*s.H
'
+
s.R);
%
Correction based on observation: s.x
=
s.x
+
K
*
(s.z
-
s.H
*
s.x); s.P
=
s.P
-
K
*
s.H
*
s.P;
%
Note that the desired result, which
is
an improved estimate
%
of the sytem state vector x and its covariance P, was obtained
%
in
only five lines of code, once the system was defined. (That
'
s
%
how simple the discrete Kalman filter
is
to use.) Later,
%
we
'
ll discuss how to deal with nonlinear systems.
end
后来不过瘾，自己写了一个，没想到稍微改了一改竟然成功了，效果还不错
% 状态
% xk=A•xk-1+B•uk+wk
% zk=H•xk+vk,
% p(w) ~ N(0,Q)
% p(v) ~ N(0,R),
% 预测
% x'k=A•xk+B•uk
% P'k=A•P(k-1)*AT + Q
% 修正
% Kk=P'k•HT•(H•P'k•HT+R)-1
% xk=x'k+Kk•(zk-H•x'k)
% Pk=(I-Kk•H)•P'k
%要注意的是：必须把系统状态和kalman滤波器内部预测的状态分开function Test
A=[1 0.1;0 1];
B=0;
Xp=rand(2,1)*0.1;
X=[0 0]';
H=[1 0];
Q=eye(2)*1e-5;
R=eye(1)*0.1;
P=eye(2);% P'(k)
angle=[];
angle_m=[];
angle_real=[];
for i=1:500
angle_real=[angle_real X(1)]; %实际角度
[Xp,P]=Predict(A,Xp,P,Q);
X=A*X+rand(2,1)*1e-5;
z_m=H*X+rand(1,1)*0.1-0.05;
angle_m=[angle_m z_m(1)]; %测量的角度
[Xp,P]=Correct(P,H,R,X,z_m);
angle=[angle Xp(1)]; %预测的角度
end
t=1:500;
plot(t,angle,'r',t,angle_m,'g',t,angle_real,'b')
legend('预测值','测量值','实际值')
figure
plot(t,angle-angle_real,'r',t,angle_m-angle_real,'g')
legend('滤波后的误差','测量的误差')
title('误差分析')
xlabel('time');
ylabel('error');
function [Xk,Pk]=Predict(A,Xk,Pk_1,Q)
Xk=A*Xk;
Pk=A*Pk_1*A'+Q;
function [Xk,Pk]=Correct(Pk,H,R,Xk,zk)
Kk=Pk * H' * inv(H * Pk * H' + R);
Xk=Xk+ Kk*(zk-H*Xk);
Pk=(eye(size(Pk,1)) - Kk*H)*Pk;