例6.11
利用卡尔曼滤波估计一个未知常数
题目：
设已知一个未知常数x 的噪声观测集合，已知噪声v(n)的均值为零，
方差为
，v(n)与x 不相关，试用卡尔曼滤波估计该常数
题目分析：
回忆Kalman 递推估计公式
由于已知x 为一常数，即不随时间n 变化，因此可以得到： 状态方程： x(n)=x(n-1)
观测方程： y(n)=x(n)+v(n)
得到A(n)=1,C(n)=1, , 将A(n)=1,代入迭代公式
得到：P(n|n-1)=P(n-1|n-1)
用P(n-1)来表示P(n|n-1)和P(n-1|n-1),这是卡尔曼增益表达式变为
从而 2v σ1ˆˆ(|1)(1)(1|1)(|1)(1)(1|1)(1)()()(|1)()[()(|1)()()]ˆˆˆ(|)(|1)()[()()(|1)](|)[()()](|1)H w H H v x n n A n x n n P n n A n P n n A n Q n K n P n n C n C n P n n C n Q n x
n n x n n K n y n C n x n n P n n I K n C n P n n --=----=----+=--+=-+--=--2()v v
Q n σ=()0w Q n =(|1)(1)(1|1)(1)()H w P n n A n P n n A n Q n -=----+21
()(|1)[(|1)]v K n P n n P n n σ-=--+22(1)()[1()](1)(1)v
v P n P n K n P n P n σσ-=--=-+
利用递归的方法
...
得到一般的P(n)计算式
将上式代入卡尔曼增益K(n)计算式得：
最后得到离散卡尔曼滤波器计算式：
22(0)(1)(0)v v P P P σσ=+2222(1)(0)(2)(1)2(0)v v v v P P P P P σσσσ==++2222(2)(0)(3)(2)3(0)v v v v
P P P P P σσσσ==++22(0)()(0)v v P P n nP σσ=+22(1)(0)()(1)(0)v v P n P K n P n nP σσ-==-++2(0)ˆˆˆ()(1)[()(1)](0)v P x n x n y n x n nP σ=-+--+
Matlab实现
clear
N=200;
%w(n)=0
A=1;
x(1)=50;%initial state
for k=2:N;
x(k)=A*x(k-1);
end
V=randn(1,N);
rv_sd=std(V);
Rvv=rv_sd.^2;
%Rxx no use,Rww is 0
C=1;
Y=C*x+V;
%Kalman filter to estimate
p(1)=10;%initial MSE value
s(1)=100;%initial estimated value
for t=2:N;
p(t)=p(1).*Rvv/(t*p(1)+Rvv);
k(t)=p(1)/(t*p(1)+Rvv);
s(t)=A*s(t-1)+k(t)*(Y(t)-A*C*s(t-1)); end
t=1:N;
figure(1)
plot(t,k); title('Kalman Gain');
figure(2)
plot(t,s,'r',t,Y,'g',t,x,'b');
legend('estimated','measured','state'); figure(3)
plot(t,p); title('Mean Square of Error');
仿真结果
