Kalman滤波 Matlab仿真

先列出KalMan的5个基本方程

公式1: 得到过度时刻  (k|k-1) 的估计值

公式2: 得到过度时刻的协方差, Q为噪声偏差值,由用户定义

公式3: 估计本时刻的最优值,其中H为测量值对真实值的增益,比如假定真实值为100, 测量值为90,则测量值对真实值的增益为0.9

公式4: 计算本时刻的卡尔曼增益H'是H的转置矩阵,R为估计值的偏差值

公式5: 更新协方差

那么做卡尔曼滤波是需要初始化的参数有:

1. 初始化两个0时刻值,这两个值不用太在意,因为kalman会逐渐对其收敛修正

0时刻的估计值,需要赋一个初值   x(0)

0时刻的协方差,需要赋一个初值  p(0) 

2. 系统相关的配置值,这些值决定了kalman的滤波性能

A 状态转移矩阵,用1唯的可理解为上一时刻到下一时刻的值改变了多少,A为零表示下一时刻和上一时刻的估计值相同,

若下一时刻同上一时刻的估计值认为相同则A可设为1(一维的哦)

B,u(k)是控制量,若无控制量则可不用

H 测量值和真实值的测量矩阵,举个例子,若电机实际转动了100度,而测量值为95度,测量值比真实值的比为0.95,

若认了测量值测得值与真实值没有差别,则H可设为1

Q 真实值被叠加的高斯白噪声的偏差,这个值表征了噪声的抖动的标准偏差,需要用户根据实际的噪声情况标定

R    测量值的高斯白噪声,同上需要用户标定

%%%%%%%%%%%%%%%%%%%%%%%%%%%     Matlab程序如下   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear
N=200;      %采样点个数设定


%得到高斯白噪声
w(1)=0;
w=randn(1,N)
x(1)=0;
a=1;        %可理解为当前值到下一个值的增益,1即为不变


%得到加入了高斯白噪声的信号
for k=2:N;
x(k)=a*x(k-1)+w(k-1);       
end


%得到测量值的高斯白噪声的方差
V=randn(1,N);
q1=std(V);


Rvv=q1.^2;
q2=std(x);
Rxx=q2.^2;


%得到高斯白噪声的方差
q3=std(w);
Rww=q3.^2;      


%得到测量值Y
c=0.2;      %可理解为测量值是实际值的增益
Y=c*x+V;


p(1)=0;
s(1)=0;


for t=2:N;
p1(t)=a.^2*p(t-1)+Rww;                  %计算当前协方差
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);          %b(t)为计算的到的卡尔曼增益
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));   %估计值最优值
p(t)=p1(t)-c*b(t)*p1(t);                %更新协方差       
end


%显示波形
t=1:N;
plot(t,s,'r',t,Y,'g',t,x,'b');
xlabel('采样时刻');
ylabel('值');
legend('Kalman滤波值','测量值','实际值');