卡尔曼推导流程记录和一个matlab例程

clc
clear all;
close all;

%初始化参数
delta_t = 0.1; %采样时间间隔
t = 0:delta_t:10;
N = length（t）;
sz = [2N];
g = 10; %加速度大小，对应卡尔曼滤波中的控制向量
q = 10; %系统位置噪声方差
Q = [q 0;0 0]; %系统噪声协方差矩阵，大小为2X2（系统状态中只包含位置、速度两个分量）
r = 5; %测量位置噪声方差
R = [r]; %测量噪声协方差，大小为1X1（只测量了位置）
A = [1 delta_t; 0 1]; %状态转换矩阵，大小为2X2
B = [1/2*delta_t^2; delta_t]; %控制转换矩阵，大小为2X1
H = [10]; %观测转换矩阵，大小为1X2

n = size（Q）;
m = size（R）;

P = zeros（n）; %估计值和真实值的误差协方差矩阵
Qn = zeros（1N）; %系统噪声导致的小车位置偏差累积
Qn（1） = q*randn;
for i = 2:1:N
    Qn（i） = Qn（i-1）+q*randn; %每一个dt内位置噪声满足高斯分布
end

x = 1/2*g*t.^2 + Qn（int32（t/delta_t+1））; %真实值
z = x + r.*randn（1N）; %加入观测噪声的观测值

xhat = zeros（sz）; %估计值
xhatminus = zeros（sz）; %预测值
Pminus = zeros（n）; %预测值与真实值的误差协方差矩阵
K = zeros（n（1）m（1））; %卡尔曼增益矩阵
I = eye（n）;

for k = 9:N %这里我们从时刻9处才开始计算，即小车已经运动了一段时间
    xhatminus（:k） = A*xhat（:k-1） + B*g; A*xhat（:k-1）+B*g;
    Pminus = A*P*A‘+Q;
    K = Pminus*H‘/（H*Pminus*H‘+R）;
    xhat（:k） = xhatminus（:k）+K*（z（k）-H*xhatminus（:k））;
    P = （I-K*H）*Pminus;
end
 
figure（1）
plot（tz）
hold on
plot（txhat（1:）‘r-‘）
plot（txhatminus（1:）‘k-‘）;
hold on
plot（tx（1:）‘g-‘）
legend（‘z‘‘xhat‘‘xhatminus‘‘x‘）;
xlabel（‘Iteration‘）;