这个卡尔曼滤波程序哪位大哥可以帮我解释一下？

clear

N=200%取200个数

w(1)=0

w=randn(1,N)%产生一个1×N的行向量，第一个数为0，w为过程噪声（其和后边的v在卡尔曼理论里均为高斯白噪声）

x(1)=0%状态x初始值

a=1%a为状态转移阵，此程序简单起见取1

for k=2:N

x(k)=a*x(k-1)+w(k-1) %系统状态方程，k时刻的状态等于k-1时刻状态乘以状态转移阵加噪声（此处忽略了系统的控制量）

end

V=randn(1,N)%测量噪声

q1=std(V)

Rvv=q1.^2

q2=std(x)

Rxx=q2.^2%此方程未用到Rxx

q3=std(w)

Rww=q3.^2%Rvv、Rww分别为过程噪声和测量噪声的协方差(此方程只取一组数方差与协方差相同)

c=0.2

Y=c*x+V%量测方差，c为量测矩阵，同a简化取为一个数

p(1)=0%初始最优化估计协方差

s(1)=0%s(1)表示为初始最优化估计

for t=2:N

p1(t)=a.^2*p(t-1)+Rww%p1为一步估计的协方差，此式从t-1时刻最优化估计s的协方差得到t-1时刻到t时刻一步估计的协方差

b(t)=c*p1(t)/(c.^2*p1(t)+Rvv)%b为卡尔曼增益，其意义表示为状态误差的协方差与量测误差的协方差之比(个人见解)

s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1))%Y(t)-a*c*s(t-1)称之为新息，是观测值与一步估计得到的观测值之差，此式由上一时刻状态的最优化估计s(t-1)得到当前时刻的最优化估计s(t)

p(t)=p1(t)-c*b(t)*p1(t)%此式由一步估计的协方差得到此时刻最优化估计的协方差

end

t=1:N

plot(t,s,'r',t,Y,'g',t,x,'b')%作图，红色为卡尔曼滤波，绿色为量测，蓝色为状态

%整体来说，此卡尔曼程序就是一个循环迭代的过程，给出初始的状态x和协方差p，得到下一时刻的x和p，循环带入可得到一系列的最优的状态估计值，此方法通常用于目标跟踪和定位。

%本人研究方向与此有关，有兴趣可以交流下。

这里的Pdot是一个中间变量，你只看几个步骤是不可能会懂的，最好要全部一起看，下面是纤细步骤：

这里用到的kalman主要分为5个步骤：

(1) X=A*X+B*angular_speed_m

(2) P=A*P*A'+Q

(3) X=X-KG(Z-H*X)

(4) K=P*H'(H*P*H'+R)

(5) (I-KG*H)*P

对矩阵P的更新只有 P=A*P*A'+QP,A,Q都是矩阵

A=1 -TS

0 1

Q=q_acce 0

0 q_gyro

单片机目前肯定是不能计算矩阵啦，说以就自己算矩阵乘法，加法(线性代数)

P[0][0]=(KAL_A[0][0]*P[0][0]+KAL_A[0][1]*P[1][0])*KAL_A[0][0]+(KAL_A[0][0]*P[0][1]+KAL_A[0][1]*P[1][1])*KAL_A[0][1]+KAL_Q[0][0]/////////////////////

P[0][1]=(KAL_A[0][0]*P[0][0]+KAL_A[0][1]*P[1][0])*KAL_A[1][0]+(KAL_A[0][0]*P[0][1]+KAL_A[0][1]*P[1][1])*KAL_A[1][1]+KAL_Q[0][1]////////////////////

P[1][0]=(KAL_A[1][0]*P[0][0]+KAL_A[1][1]*P[1][0])*KAL_A[0][0]+(KAL_A[1][0]*P[0][1]+KAL_A[1][1]*P[1][1])*KAL_A[0][1]+KAL_Q[1][0]////////////////////

P[1][1]=(KAL_A[1][0]*P[0][0]+KAL_A[1][1]*P[1][0])*KAL_A[1][0]+(KAL_A[1][0]*P[0][1]+KAL_A[1][1]*P[1][1])*KAL_A[1][1]+KAL_Q[1][1]////////////////////

然后把等于1或0的式子直接去掉

P[0][0]=(P[0][0]+KAL_A[0][1]*P[1][0])+(P[0][1]+KAL_A[0][1]*P[1][1])*KAL_A[0][1]+q_acce// a

P[0][1]=P[0][1]+KAL_A[0][1]*P[1][1]//b

P[1][0]=P[1][0]+P[1][1]*KAL_A[0][1]//c

P[1][1]=P[1][1]+q_gyro// d

下面是你写的

P[0][0] += Pdot[0] * dt

P[1][1] += Pdot[3] * dt

Pdot[0] = Q_angle - P[0][1] - P[1][0]

Pdot[3] = Q_gyro

简化，约分后

P[0][0] +=（Q_angle - P[0][1] - P[1][0]）* dt； >>对应上面的a

P[1][1] +=Q_gyro * dt >>对应上面的d

当然我自建的kalman和你看的那个版本还是有点差异，不过我的这个版本是可以用的，你看的哪个版本也可以用，效果都差不多。

我的回答就是这样，记得点赞哦！！！！

我现在也在研究kalman，这是最新发现的一个程序，我做的标注，有问题一块研究

function [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, varargin)

% Kalman filter.

% [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ...)

%

% INPUTS:

% y(:,t) - the observation at time t 在时间t的观测

% A - the system matrix A 系统矩阵

% C - the observation matrix C 观测矩阵

% Q - the system covariance Q 系统协方差

% R - the observation covariance R 观测协方差

% init_x - the initial state (column) vector init_x 初始状态（列）向量

% init_V - the initial state covariance init_V初始状态协方差

%

% OPTIONAL INPUTS (string/value pairs [default in brackets]) 选择性输入（字符串/值 对【默认在括号中】）

% 'model' - model(t)=m means use params from model m at time t [ones(1,T) ] 在时间t，m意味着利用 m模型参数 [ones(1,T) ]

%

% In this case, all the above matrices take an additional final

% dimension, 在这种情况下,上述矩阵采用附加的维数

% i.e., A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m). 例如

% However, init_x and init_V are independent of model(1).

% init_x and init_V相对于模型1是独立的

% 'u' - u(:,t) the control signal at time t [ [] ]

% 在时间t的控制信号

% 'B' - B(:,:,m) the input regression matrix for model m

% 对于模型m的，输入回归矩阵

% OUTPUTS (where X is the hidden state being estimated) 输出（其中X是被估计的隐藏状态）

% x(:,t) = E[X(:,t) | y(:,1:t)]

% V(:,:,t) = Cov[X(:,t) | y(:,1:t)]

% VV(:,:,t) = Cov[X(:,t), X(:,t-1) | y(:,1:t)] t >= 2

% loglik = sum{t=1}^T log P(y(:,t))

%

% If an input signal is specified, we also condition on it: 如果一个输入信号是特定的,我们的条件

% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t)]

% If a model sequence is specified, we also condition on it:

% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t), m(1:t)]

[os T] = size(y)

ss = size(A,1)% size of state space

% set default params

model = ones(1,T)

u = []

B = []

ndx = []

args = varargin

nargs = length(args)

for i=1:2:nargs

switch args{i}

case 'model', model = args{i+1}

case 'u', u = args{i+1}

case 'B', B = args{i+1}

case 'ndx', ndx = args{i+1}

otherwise, error(['unrecognized argument ' args{i}])

end

end

x = zeros(ss, T)

V = zeros(ss, ss, T)

VV = zeros(ss, ss, T)

loglik = 0

for t=1:T

m = model(t)

if t==1

%prevx = init_x(:,m)

%prevV = init_V(:,:,m)

prevx = init_x

prevV = init_V

initial = 1

else

prevx = x(:,t-1)

prevV = V(:,:,t-1)

initial = 0

end

if isempty(u)

[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...

kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, 'initial', initial)

else

if isempty(ndx)

[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...

kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, ...

'initial', initial, 'u', u(:,t), 'B', B(:,:,m))

else

i = ndx{t}

% copy over all elementsonly some will get updated

x(:,t) = prevx

prevP = inv(prevV)

prevPsmall = prevP(i,i)

prevVsmall = inv(prevPsmall)

[x(i,t), smallV, LL, VV(i,i,t)] = ...

kalman_update(A(i,i,m), C(:,i,m), Q(i,i,m), R(:,:,m), y(:,t), prevx(i), prevVsmall, ...

'initial', initial, 'u', u(:,t), 'B', B(i,:,m))

smallP = inv(smallV)

prevP(i,i) = smallP

V(:,:,t) = inv(prevP)

end

end

loglik = loglik + LL

end

---