递推最小二乘估计理论

递推最小二乘是卡尔曼滤波的重要基础，关于最小二乘理论可以参见之前的文章（最小二乘估计（Least squares estimation）），在此只给出递推最小二乘估计的步骤

计算增益
Kk=Pk−1HkT(HkPk−1HkT+Rk)−1K_k=P_{k-1}H_k^T(H_kP_{k-1}H_k^T+R_k)^{-1}Kk=Pk−1HkT(HkPk−1HkT+Rk)−1
估计值更新
x^k=x^k−1+Kk(yk−Hkx^k−1)\hat{x}_k=\hat{x}_{k-1}+K_{k}(y_k-H_{k}\hat{x}_{k-1})x^k=x^k−1+Kk(yk−Hkx^k−1)
协方差更新
Pk=(I−KkHk)Pk−1(I−KkHk)T+KkRkKkTP_k=(I-K_kH_k)P_{k-1}(I-K_kH_k)^T+K_kR_kK_k^TPk=(I−KkHk)Pk−1(I−KkHk)T+KkRkKkT

仿真背景

仿真场景为量一件物件的长和宽，真实物件的长宽分别为2米和3米，一共测量五十次，假定量测噪声为零均值的高斯噪声，且每次量测噪声之间是不相关的。根据更新的量测绘制出递推最小二乘估计的过程动图。

Matlab程序

clc;clear;close all
X_0 = [2;3];
N = 50;
H = 1;
R = [1 0;0 1];
n = zeros(2,N);
Y = zeros(2,N);
X_e = zeros(2,N);
P_e = zeros(2,2,N);
K = zeros(2,2,N);
error = zeros(2,N);
h1 = subplot(221);
h2 = subplot(222);
h3 = subplot(223);
h4 = subplot(224);
for i = 1:Nn(:,i) = mvnrnd([0;0],R,1);Y(:,i) = H*X_0 + n(:,i);plot(h1,Y(1,1:i),Y(2,1:i),'m.');title(h1,'位置观测和估计更新');axis(h1,[0 4 1 5]);hold(h1,'on')if i == 1X_e(:,1) = Y(:,1);P_e(:,:,1) = R;plot(h1,X_e(1,1),X_e(2,1),'bo');error(:,1) = X_e(:,1) - X_0;elseK(:,:,i) = P_e(:,:,i-1)*H'/(H*P_e(:,:,i-1)*H'+R);plot(h2,i,K(1,1,i),'b.');axis(h2,[0 N 0 0.6]);hold(h2,'on');X_e(:,i) = X_e(:,i-1) + K(:,:,i)*(Y(:,i)-H*X_e(:,i-1));P_e(:,:,i) = (eye(2)-K(:,:,i)*H)*P_e(:,:,i-1)*(eye(2)-K(:,:,i)*H)'+K(:,:,i)*R*K(:,:,i)';plot(h1,X_e(1,i),X_e(2,i),'bo');title(h2,'横向位置校正增益');error(:,i) = X_e(:,i) - X_0;plot(h3,1:i,abs(error(1,1:i)),'b-','LineWidth',1.2);axis(h3,[0 N 0 2]);title(h3,'横向位置估计误差');hold(h3,'on');plot(h4,1:i,abs(error(2,1:i)),'b-','LineWidth',1.2);axis(h4,[0 N 0 2]);title(h4,'纵向位置估计误差');hold(h4,'on');endhold(h1,'off')F = getframe(gcf);im = frame2im(F);[I,map] = rgb2ind(im,256);if i == 1imwrite(I,map,'RLS.gif','GIF', 'Loopcount',inf,'DelayTime',0.3);elseimwrite(I,map,'RLS.gif','GIF','WriteMode','append','DelayTime',0.3);end
end

仿真动图

注意到增益的计算是可以离线计算的，给定了初始协方差矩阵，后续的增益也就确定了。

递推最小二乘估计Matlab仿真相关推荐

基本最小二乘到递推最小二乘
基本最小二乘到递推最小二乘基本最小二乘(LS) 先导知识: 从函数出发残差梳理推导基本思想: 开始推导递推最小二乘法背景前N个输入输出数据开始递推递推优化结论 Matlab 示例 ...
matlab偏差补偿最小二乘,基于偏差补偿递推最小二乘的Hammerstein-Wiener模型辨识
自动化学报 2010年第1期第36卷第1期2010年1月自动化学报 ACTAAUTOMATICASINICA Vol.36,No.1January,2010 基于偏差补偿递推最小二乘的Hamme ...
递推最小二乘辨识平面双机械臂Matlab代码
本代码是我的毕设里面的一部分,首先是将SCARA机器人简化成为简单的平面双机械臂模型,对其进行动力学建模,由于SCARA机器人的特性,动力学方程中的重力项可以忽略,最后将动力学方程化简成为AX=B的形 ...
递推最小二乘RLS推导
批次的最小二乘是给定一批数据比如 {yi∈R,xi∈Rn,i=1,...,N}\{y_i\in\mathbb{R},x_i\in \mathbb{R}^n,i=1,...,N\}{yi∈R,xi∈ ...
序贯最小二乘平差 / 递推最小二乘平差公式推导
文章目录 Part.I 预备知识 Chap.I 基础概念 Chap.II 符号含义 Part.II 递推最小二乘平差原理 Chap.I 递推公式 Chap.II 递推公式推导 Chap.III 一图流 ...
递推最小二乘遗忘因子法(Recursive Forgetting Factor, RFF)
在普通的递推最小二乘算法中,随着数据的不断到来,显然矩阵XTXX^TXXTX中的元素会变得越来越大,而矩阵PkP_kPk作为XTXX^TXXTX的逆矩阵,则会逐渐趋于零,这时,模型将无法继续更新或者 ...
matlab经典music,基于经典music的doa估计matlab仿真.doc
基于经典music的doa估计matlab仿真.doc 信息与通信工程学院阵列信号处理实验报告(基于经典MUSIC的DOA估计MATLAB仿真)学号XXXXXX专业XXXXXX学生姓名XXX任课教师X ...
随机信号处理AR模型Yule_Walker方程直接解法和Levinson_Durbin递推法的MATLAB与Python实现
AR模型 AR模型的系统函数H(z)可以表示为: 我们的目的就是要求解系统函数的参数a和增益G. Yule_Walker方程矩阵形式根据生成的矩阵,可以解出p个参数 ,再根据自相关函数,可以求出系 ...
使用ESPRIT,LS-ESPRIT,Music以及Root-Music四种算法进行角度估计matlab仿真
目录一.理论基础二.核心程序三.测试结果一.理论基础 1.1ESPRIT ESPRIT算法全称为:Estimation of Signal Parameters using Rotationa ...

递推最小二乘估计Matlab仿真